def test_MoveHand(self): """Test MoveHandAction.""" self.robot.arm = self.robot.GetManipulator('arm') self.robot.hand = self.robot.arm.GetEndEffector() prpy.bind_subclass(self.robot.hand, DummyHand, manipulator=self.robot.arm) action = MoveHandAction(self.robot.hand, [0.7], self.robot.hand.GetIndices()[:1]) return self._action_helper(action)
def initialize(env_path=None, attach_viewer=False, **kw_args): from adarobot import ADARobot from util import AdaPyException, find_adapy_resource prpy.logger.initialize_logging() # Create the environment. env = openravepy.Environment() if env_path is not None: if not env.Load(env_path): raise IOError( 'Unable to load environment "{:s}".'.format(env_path)) # Use or_urdf to load ADA from URDF and SRDF. with env: or_urdf = openravepy.RaveCreateModule(env, 'urdf') ada_name = or_urdf.SendCommand('load {:s} {:s}'.format( URDF_PATH, SRDF_PATH)) robot = env.GetRobot(ada_name) if robot is None: raise AdaPyException('Failed loading ADA with or_urdf.') # Bind AdaPy-specific functionality on the robot. prpy.bind_subclass(robot, ADARobot, **kw_args) # Start by attempting to load or_rviz. if attach_viewer == True: attach_viewer = 'rviz' env.SetViewer(attach_viewer) # Fall back on qtcoin if loading or_rviz failed if env.GetViewer() is None: logger.warning('Loading rviz failed. Falling back on qt_coin.') attach_viewer = 'qtcoin' if attach_viewer and env.GetViewer() is None: #Using qtcoin as or_rviz gives seg fault attach_viewer = 'qtcoin' env.SetViewer(attach_viewer) if env.GetViewer() is None: raise AdaPyException( 'Failed creating viewer of type "{:s}".'.format(attach_viewer)) # Remove the ROS logging handler again. It might have been added when we # loaded or_rviz. prpy.logger.remove_ros_logger() import adapy.action # register actions import adapy.tsr # register TSR libraries robot.actions = prpy.action.ActionLibrary() return env, robot
def __init__(self): Robot.__init__(self, robot_name='archie') # Set up ikfast for the robot. May want to add this to a # Manipulator class init method instead. self.ikmodel = ( openravepy.databases.inversekinematics.InverseKinematicsModel( self, iktype=openravepy.IkParameterization.Type.Transform6D)) if not self.ikmodel.load(): self.ikmodel.autogenerate() self.arm = self.GetManipulator('j2s7s300') bind_subclass(self.arm, ArchieManipulator) self.planner = SnapPlanner() self.arm.sim_controller = self.AttachController( name=self.arm.GetName(), args='IdealController', dof_indices=self.arm.GetArmIndices(), affine_dofs=0, simulated=True) self.SetActiveDOFs(self.arm.GetIndices())
def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim, left_hand_sim, right_hand_sim, left_ft_sim, head_sim, talker_sim, segway_sim, perception_sim): from prpy.util import FindCatkinResource Robot.__init__(self, robot_name='herb') # Controller setup self.controller_manager = None self.controllers_always_on = [] self.full_controller_sim = (left_arm_sim and right_arm_sim and left_ft_sim and right_ft_sim and left_hand_sim and right_hand_sim and head_sim) if not self.full_controller_sim: # any non-simulation requires ros and the ros_control stack import rospy from ros_control_client_py import (ControllerManagerClient, JointStateClient) if not rospy.core.is_initialized(): raise RuntimeError('rospy not initialized. ' 'Must call rospy.init_node()') # update openrave state from /joint_states self._jointstate_client = JointStateClient( self, topic_name='/joint_states') self.controller_manager = ControllerManagerClient() self.controllers_always_on.append('joint_state_controller') # Convenience attributes for accessing self components. self.left_arm = self.GetManipulator('left') self.right_arm = self.GetManipulator('right') self.head = self.GetManipulator('head') self.left_arm.hand = self.left_arm.GetEndEffector() self.right_arm.hand = self.right_arm.GetEndEffector() self.left_hand = self.left_arm.hand self.right_hand = self.right_arm.hand self.manipulators = [self.left_arm, self.right_arm, self.head] # Dynamically switch to self-specific subclasses. prpy.bind_subclass(self.left_arm, WAM, sim=left_arm_sim, namespace='/left') prpy.bind_subclass(self.right_arm, WAM, sim=right_arm_sim, namespace='/right') prpy.bind_subclass(self.head, HERBPantilt, sim=head_sim, owd_namespace='/head/owd') prpy.bind_subclass(self.left_arm.hand, BarrettHand, sim=left_hand_sim, manipulator=self.left_arm, bhd_namespace='/left', ft_sim=left_ft_sim) prpy.bind_subclass(self.right_arm.hand, BarrettHand, sim=right_hand_sim, manipulator=self.right_arm, bhd_namespace='/right', ft_sim=right_ft_sim) self.base = HerbBase(sim=segway_sim, robot=self) # Set HERB's acceleration limits. These are not specified in URDF. accel_limits = self.GetDOFAccelerationLimits() accel_limits[self.head.GetArmIndices()] = [ 2. ] * self.head.GetArmDOF() accel_limits[self.left_arm.GetArmIndices()] = [ 2. ] * self.left_arm.GetArmDOF() accel_limits[self.right_arm.GetArmIndices()] = [ 2. ] * self.right_arm.GetArmDOF() self.SetDOFAccelerationLimits(accel_limits) # Determine always-on controllers # hand controllers if not left_hand_sim: self.controllers_always_on.append('left_hand_controller') if not right_hand_sim: self.controllers_always_on.append('right_hand_controller') # force/torque controllers if not left_ft_sim or not right_ft_sim: self.controllers_always_on.append('force_torque_sensor_controller') if not left_ft_sim: self.controllers_always_on.append('left_tare_controller') if not right_ft_sim: self.controllers_always_on.append('right_tare_controller') # Set default manipulator controllers in sim only # NOTE: head is ignored until TODO new Schunk head integrated if left_arm_sim: self.left_arm.sim_controller = self.AttachController(name=self.left_arm.GetName(), args='IdealController', dof_indices=self.left_arm.GetArmIndices(), affine_dofs=0, simulated=True) if right_arm_sim: self.right_arm.sim_controller = self.AttachController(name=self.right_arm.GetName(), args='IdealController', dof_indices=self.right_arm.GetArmIndices(), affine_dofs=0, simulated=True) # load and activate initial controllers if self.controller_manager is not None: self.controller_manager.request( self.controllers_always_on).switch() # Support for named configurations. import os.path self.configurations.add_group('left_arm', self.left_arm.GetArmIndices()) self.configurations.add_group('right_arm', self.right_arm.GetArmIndices()) self.configurations.add_group('head', self.head.GetArmIndices()) self.configurations.add_group('left_hand', self.left_hand.GetIndices()) self.configurations.add_group('right_hand', self.right_hand.GetIndices()) configurations_path = FindCatkinResource('herbpy', 'config/configurations.yaml') try: self.configurations.load_yaml(configurations_path) except IOError as e: raise ValueError('Failed laoding named configurations from "{:s}".'.format( configurations_path)) # Hand configurations from prpy.named_config import ConfigurationLibrary for hand in [ self.left_hand, self.right_hand ]: hand.configurations = ConfigurationLibrary() hand.configurations.add_group('hand', hand.GetIndices()) if isinstance(hand, BarrettHand): hand_configs_path = FindCatkinResource('herbpy', 'config/barrett_preshapes.yaml') try: hand.configurations.load_yaml(hand_configs_path) except IOError as e: raise ValueError('Failed loading named hand configurations from "{:s}".'.format( hand_configs_path)) else: logger.warning('Unrecognized hand class. Not loading named configurations.') # Initialize a default planning pipeline. from prpy.planning import ( FirstSupported, MethodMask, Ranked, Sequence, ) from prpy.planning import ( CBiRRTPlanner, CHOMPPlanner, GreedyIKPlanner, IKPlanner, NamedPlanner, SBPLPlanner, SnapPlanner, TSRPlanner, VectorFieldPlanner ) # TODO: These should be meta-planners. self.named_planner = NamedPlanner() self.ik_planner = IKPlanner() # Special-purpose planners. self.snap_planner = SnapPlanner() self.vectorfield_planner = VectorFieldPlanner() self.greedyik_planner = GreedyIKPlanner() # General-purpose planners. self.cbirrt_planner = CBiRRTPlanner() # Trajectory optimizer. try: from or_trajopt import TrajoptPlanner self.trajopt_planner = TrajoptPlanner() except ImportError: self.trajopt_planner = None logger.warning('Failed creating TrajoptPlanner. Is the or_trajopt' ' package in your workspace and built?') try: self.chomp_planner = CHOMPPlanner() except UnsupportedPlanningError: self.chomp_planner = None logger.warning('Failed loading the CHOMP module. Is the or_cdchomp' ' package in your workspace and built?') if not (self.trajopt_planner or self.chomp_planner): raise PrPyException('Unable to load both CHOMP and TrajOpt. At' ' least one of these packages is required.') actual_planner = Sequence( # First, try the straight-line trajectory. self.snap_planner, # Then, try a few simple (and fast!) heuristics. self.vectorfield_planner, #self.greedyik_planner, # Next, try a trajectory optimizer. self.trajopt_planner or self.chomp_planner ) self.planner = FirstSupported( Sequence(actual_planner, TSRPlanner(delegate_planner=actual_planner), self.cbirrt_planner), # Special purpose meta-planner. NamedPlanner(delegate_planner=actual_planner), ) from prpy.planning.retimer import HauserParabolicSmoother self.smoother = HauserParabolicSmoother(do_blend=True, do_shortcut=True) self.retimer = HauserParabolicSmoother(do_blend=True, do_shortcut=False, blend_iterations=5, blend_radius=0.4) self.simplifier = None # Base planning from prpy.util import FindCatkinResource planner_parameters_path = FindCatkinResource('herbpy', 'config/base_planner_parameters.yaml') self.sbpl_planner = SBPLPlanner() try: with open(planner_parameters_path, 'rb') as config_file: import yaml params_yaml = yaml.load(config_file) self.sbpl_planner.SetPlannerParameters(params_yaml) except IOError as e: raise ValueError('Failed loading base planner parameters from "{:s}".'.format( planner_parameters_path)) self.base_planner = self.sbpl_planner # Create action library from prpy.action import ActionLibrary self.actions = ActionLibrary() # Register default actions and TSRs import herbpy.action import herbpy.tsr # Setting necessary sim flags self.talker_simulated = talker_sim self.segway_sim = segway_sim # Set up perception self.detector=None if perception_sim: from prpy.perception import SimulatedPerceptionModule self.detector = SimulatedPerceptionModule() else: from prpy.perception import ApriltagsModule try: kinbody_path = prpy.util.FindCatkinResource('pr_ordata', 'data/objects') marker_data_path = prpy.util.FindCatkinResource('pr_ordata', 'data/objects/tag_data.json') self.detector = ApriltagsModule(marker_topic='/apriltags_kinect2/marker_array', marker_data_path=marker_data_path, kinbody_path=kinbody_path, detection_frame='head/kinect2_rgb_optical_frame', destination_frame='herb_base', reference_link=self.GetLink('/herb_base')) except IOError as e: logger.warning('Failed to find required resource path. ' \ 'pr_ordata package cannot be found. ' \ 'Perception detector will not be loaded.' \ '\n{}'.format(e)) if not self.talker_simulated: # Initialize herbpy ROS Node import rospy if not rospy.core.is_initialized(): raise RuntimeError('rospy not initialized. ' 'Must call rospy.init_node()') import talker.msg from actionlib import SimpleActionClient self._say_action_client = SimpleActionClient('say', talker.msg.SayAction)
def __init__(self, arm_sim, hand_sim, ft_sim, segway_sim, ): from prpy.util import FindCatkinResource Robot.__init__(self, robot_name='BarrettWAM') # Convenience attributes for accessing self components. self.arm = self.GetManipulator('arm') self.arm.hand = self.arm.GetEndEffector() self.hand = self.arm.hand # 'handbase' self.manipulators = [ self.arm ] # Dynamically switch to self-specific subclasses. prpy.bind_subclass(self.arm, WAM, sim=arm_sim, owd_namespace='/arm/owd') # namespace must be set, even for sim prpy.bind_subclass(self.arm.hand, BarrettHand, sim=hand_sim, manipulator=self.arm, owd_namespace='/arm/owd', bhd_namespace='/arm/bhd', ft_sim=ft_sim) self.base = WAMBase(sim=segway_sim, robot=self) #self.base = WAMBase(sim=base_sim, robot=self) # Set WAM acceleration limits. These are not specified in URDF. accel_limits = self.GetDOFAccelerationLimits() accel_limits[self.arm.GetArmIndices()] = [ 2. ] * self.arm.GetArmDOF() self.SetDOFAccelerationLimits(accel_limits) # Support for named configurations. import os.path self.configurations.add_group('arm', self.arm.GetArmIndices()) self.configurations.add_group('hand', self.hand.GetIndices()) configurations_path = FindCatkinResource('wampy', 'config/configurations.yaml') try: self.configurations.load_yaml(configurations_path) except IOError as e: raise ValueError('Failed laoding named configurations from "{:s}".'.format( configurations_path)) # Initialize a default planning pipeline. from prpy.planning import ( FirstSupported, MethodMask, Ranked, Sequence, ) from prpy.planning import ( CBiRRTPlanner, CHOMPPlanner, GreedyIKPlanner, IKPlanner, NamedPlanner, SBPLPlanner, SnapPlanner, TSRPlanner, VectorFieldPlanner ) # TODO: These should be meta-planners. self.named_planner = NamedPlanner() self.ik_planner = IKPlanner() # Special-purpose planners. self.snap_planner = SnapPlanner() self.vectorfield_planner = VectorFieldPlanner() self.greedyik_planner = GreedyIKPlanner() # General-purpose planners. self.cbirrt_planner = CBiRRTPlanner() # Trajectory optimizer. try: from or_trajopt import TrajoptPlanner self.trajopt_planner = TrajoptPlanner() except ImportError: self.trajopt_planner = None logger.warning('Failed creating TrajoptPlanner. Is the or_trajopt' ' package in your workspace and built?') try: self.chomp_planner = CHOMPPlanner() except UnsupportedPlanningError: self.chomp_planner = None logger.warning('Failed loading the CHOMP module. Is the or_cdchomp' ' package in your workspace and built?') if not (self.trajopt_planner or self.chomp_planner): raise PrPyException('Unable to load both CHOMP and TrajOpt. At' ' least one of these packages is required.') actual_planner = Sequence( # First, try the straight-line trajectory. self.snap_planner, # Then, try a few simple (and fast!) heuristics. self.vectorfield_planner, #self.greedyik_planner, # Next, try a trajectory optimizer. self.trajopt_planner or self.chomp_planner ) self.planner = FirstSupported( Sequence(actual_planner, TSRPlanner(delegate_planner=actual_planner), self.cbirrt_planner), # Special purpose meta-planner. NamedPlanner(delegate_planner=actual_planner), ) from prpy.planning.retimer import HauserParabolicSmoother self.smoother = HauserParabolicSmoother(do_blend=True, do_shortcut=True) self.retimer = HauserParabolicSmoother(do_blend=True, do_shortcut=False, blend_iterations=5, blend_radius=0.4) self.simplifier = None # Base planning from prpy.util import FindCatkinResource planner_parameters_path = FindCatkinResource('wampy', 'config/base_planner_parameters.yaml') self.sbpl_planner = SBPLPlanner() try: with open(planner_parameters_path, 'rb') as config_file: import yaml params_yaml = yaml.load(config_file) self.sbpl_planner.SetPlannerParameters(params_yaml) except IOError as e: raise ValueError('Failed loading base planner parameters from "{:s}".'.format( planner_parameters_path)) self.base_planner = self.sbpl_planner # Create action library from prpy.action import ActionLibrary self.actions = ActionLibrary() # Register default actions and TSRs import wampy.action import wampy.tsr # Setting necessary sim flags self.segway_sim = segway_sim
def initialize(robot_xml=None, env_path=None, viewer='rviz', sim=True, **kw_args): prpy.logger.initialize_logging() #Hide TrajOpt logging. os.environ.setdefault('TRAJOPT_LOG_THRESH', 'WARN') #Load plugins prpy.dependency_manager.export() RaveInitialize(True) #Create the environment env = Environment() if env_path is not None: if not env.Load(env_path): raise ValueError( 'Unable to load environment from path {:s}'.format(env_path)) #Load the URDF file into OpenRave. urdf_module = RaveCreateModule(env, 'urdf') if urdf_module is None: logger.error('Unable to load or_urdf module. Do you have or_urdf' 'built and installed in one of your Catkin workspaces?') raise ValueError('Unable to load or_urdf plugin.') urdf_uri = 'package://fetchpy/robot_description/fetch.gazebo.urdf' srdf_uri = 'package://fetchpy/robot_description/fetch.srdf' args = 'Load {:s} {:s}'.format(urdf_uri, srdf_uri) fetch_name = urdf_module.SendCommand(args) if fetch_name is None: raise ValueError('Failed loading FETCH model using or_urdf.') robot = env.GetRobot(fetch_name) if robot is None: raise ValueError( 'Unable to find robot with name "{:s}".'.format(fetch_name)) #Default to FCL. collision_checker = RaveCreateCollisionChecker(env, 'fcl') if collision_checker is not None: env.SetCollisionChecker(collision_checker) else: collision_checker = env.GetCollisionChecker() logger.warning( 'Failed creating "fcl", defaulting to the default OpenRAVE' ' collision checker. Did you install or_fcl?') #Enable Baking if it is supported try: result = collision_checker.SendCommand('BakeGetType') is_baking_suported = (result is not None) except openrave_exception: is_baking_suported = False if is_baking_suported: robot_checker_factory = BakedRobotCollisionCheckerFactory() else: robot_checker_factory = SimpleRobotCollisionCheckerFactory() logger.warning( 'Collision checker does not support baking. Defaulting to' ' the slower SimpleRobotCollisionCheckerFactory.') #Default arguments keys = [ 'arm_sim', 'arm_torso_sim', 'gripper_sim', 'head_sim', 'torso_sim', 'base_sim', 'talker_sim', 'perception_sim', 'whole_body_sim' ] for key in keys: if key not in kw_args: kw_args[key] = sim prpy.bind_subclass(robot, FETCHRobot, robot_checker_factory=robot_checker_factory, **kw_args) if sim: dof_indices, dof_values = robot.configurations.get_configuration( 'straight') robot.SetDOFValues(dof_values, dof_indices) #Start by attempting to load or_rviz. viewers = ['qtcoin', 'rviz'] if viewer is None: viewer = 'rviz' if viewer not in viewers: raise Exception( 'Failed creating viewer of type "{0:s}".'.format(viewer)) if viewer == 'rviz': env.SetViewer(viewer) if env.GetViewer() is None: logger.warning( 'Loading the RViz viewer failed. Do you have or_interactive' ' marker installed? Falling back on qtcoin.') viewer == 'qtcoin' if viewer == 'qtcoin': env.SetViewer(viewer) if viewer and env.GetViewer() is None: env.SetViewer(viewer) if env.GetViewer() is None: raise Exception( 'Failed creating viewer of type "{0:s}".'.format(viewer)) # if attach_viewer == 'True': # attach_viewer = 'qtcoin' # env.SetViewer(attach_viewer) # # Fall back on qtcoin if loading or_rviz failed # if env.GetViewer() is None: # logger.warning('Loading the RViz viewer failed. Do you have or_interactive' # ' marker installed? Falling back on qtcoin.') # attach_viewer = 'rviz' # if attach_viewer and env.GetViewer() is None: # env.SetViewer(attach_viewer) # if env.GetViewer() is None: # raise Exception('Failed creating viewer of type "{0:s}".'.format(attach_viewer)) #Remove the ROS logging handler again. It might have been added when we # loaded or_rviz. prpy.logger.remove_ros_logger() return env, robot
def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim, left_hand_sim, right_hand_sim, left_ft_sim, head_sim, talker_sim, segway_sim, perception_sim, robot_checker_factory): Robot.__init__(self, robot_name='herb') self.robot_checker_factory = robot_checker_factory # Controller setup self.controller_manager = None self.controllers_always_on = [] self.full_controller_sim = (left_arm_sim and right_arm_sim and left_ft_sim and right_ft_sim and left_hand_sim and right_hand_sim and head_sim) if not self.full_controller_sim: # any non-simulation requires ros and the ros_control stack import rospy from ros_control_client_py import ( ControllerManagerClient, JointStateClient, ) if not rospy.core.is_initialized(): raise RuntimeError('rospy not initialized. ' 'Must call rospy.init_node()') # update openrave state from /joint_states self._jointstate_client = JointStateClient( self, topic_name='/joint_states') self.controller_manager = ControllerManagerClient() self.controllers_always_on.append('joint_state_controller') # Convenience attributes for accessing self components. self.left_arm = self.GetManipulator('left') self.right_arm = self.GetManipulator('right') self.head = self.GetManipulator('head') self.left_arm.hand = self.left_arm.GetEndEffector() self.right_arm.hand = self.right_arm.GetEndEffector() self.left_hand = self.left_arm.hand self.right_hand = self.right_arm.hand self.manipulators = [self.left_arm, self.right_arm, self.head] # Dynamically switch to self-specific subclasses. prpy.bind_subclass(self.left_arm, WAM, sim=left_arm_sim, namespace='/left') prpy.bind_subclass(self.right_arm, WAM, sim=right_arm_sim, namespace='/right') prpy.bind_subclass(self.head, HERBPantilt, sim=head_sim, owd_namespace='/head/owd') prpy.bind_subclass(self.left_arm.hand, BarrettHand, sim=left_hand_sim, manipulator=self.left_arm, bhd_namespace='/left', ft_sim=left_ft_sim) prpy.bind_subclass(self.right_arm.hand, BarrettHand, sim=right_hand_sim, manipulator=self.right_arm, bhd_namespace='/right', ft_sim=right_ft_sim) self.base = HerbBase(sim=segway_sim, robot=self) # Set HERB's acceleration limits. These are not specified in URDF. accel_limits = self.GetDOFAccelerationLimits() accel_limits[self.head.GetArmIndices()] = [2.] * self.head.GetArmDOF() accel_limits[ self.left_arm.GetArmIndices()] = [2.] * self.left_arm.GetArmDOF() accel_limits[self.right_arm.GetArmIndices( )] = [2.] * self.right_arm.GetArmDOF() self.SetDOFAccelerationLimits(accel_limits) # Determine always-on controllers # hand controllers if not left_hand_sim: self.controllers_always_on.append('left_hand_controller') if not right_hand_sim: self.controllers_always_on.append('right_hand_controller') # force/torque controllers if not left_ft_sim or not right_ft_sim: self.controllers_always_on.append('force_torque_sensor_controller') if not left_ft_sim: self.controllers_always_on.append('left_tare_controller') if not right_ft_sim: self.controllers_always_on.append('right_tare_controller') # Set default manipulator controllers in sim only # NOTE: head is ignored until TODO new Schunk head integrated if left_arm_sim: self.left_arm.sim_controller = self.AttachController( name=self.left_arm.GetName(), args='IdealController', dof_indices=self.left_arm.GetArmIndices(), affine_dofs=0, simulated=True) if right_arm_sim: self.right_arm.sim_controller = self.AttachController( name=self.right_arm.GetName(), args='IdealController', dof_indices=self.right_arm.GetArmIndices(), affine_dofs=0, simulated=True) # load and activate initial controllers if self.controller_manager is not None: self.controller_manager.request( self.controllers_always_on).switch() # Support for named configurations. import os.path self.configurations.add_group('left_arm', self.left_arm.GetArmIndices()) self.configurations.add_group('right_arm', self.right_arm.GetArmIndices()) self.configurations.add_group('head', self.head.GetArmIndices()) self.configurations.add_group('left_hand', self.left_hand.GetIndices()) self.configurations.add_group('right_hand', self.right_hand.GetIndices()) configurations_path = FindCatkinResource('herbpy', 'config/configurations.yaml') try: self.configurations.load_yaml(configurations_path) except IOError as e: raise ValueError( 'Failed laoding named configurations from "{:s}".'.format( configurations_path)) # Hand configurations for hand in [self.left_hand, self.right_hand]: hand.configurations = ConfigurationLibrary() hand.configurations.add_group('hand', hand.GetIndices()) if isinstance(hand, BarrettHand): hand_configs_path = FindCatkinResource( 'herbpy', 'config/barrett_preshapes.yaml') try: hand.configurations.load_yaml(hand_configs_path) except IOError as e: raise ValueError( 'Failed loading named hand configurations from "{:s}".' .format(hand_configs_path)) else: logger.warning( 'Unrecognized hand class. Not loading named configurations.' ) # Planner. snap_planner = SnapPlanner( robot_checker_factory=self.robot_checker_factory) vectorfield_planner = VectorFieldPlanner( robot_checker_factory=self.robot_checker_factory) trajopt_planner = TrajoptPlanner( robot_checker_factory=self.robot_checker_factory) rrt_planner = OMPLPlanner( 'RRTConnect', robot_checker_factory=self.robot_checker_factory) cbirrt_planner = CBiRRTPlanner( timelimit=1., robot_checker_factory=self.robot_checker_factory) actual_planner = Sequence( snap_planner, vectorfield_planner, trajopt_planner, TSRPlanner(delegate_planner=Sequence(snap_planner, trajopt_planner), robot_checker_factory=self.robot_checker_factory), FirstSupported(rrt_planner, cbirrt_planner), ) self.planner = FirstSupported( actual_planner, NamedPlanner(delegate_planner=actual_planner), ) # Post-processor. self.smoother = HauserParabolicSmoother(do_blend=True, blend_iterations=1, blend_radius=0.4, do_shortcut=True, timelimit=0.6) self.retimer = HauserParabolicSmoother(do_blend=True, blend_iterations=1, blend_radius=0.4, do_shortcut=False) self.simplifier = None # Base planning planner_parameters_path = FindCatkinResource( 'herbpy', 'config/base_planner_parameters.yaml') self.sbpl_planner = SBPLPlanner() try: with open(planner_parameters_path, 'rb') as config_file: params_yaml = yaml.load(config_file) self.sbpl_planner.SetPlannerParameters(params_yaml) except IOError as e: raise ValueError( 'Failed loading base planner parameters from "{:s}".'.format( planner_parameters_path)) self.base_planner = self.sbpl_planner # Create action library self.actions = ActionLibrary() # Register default actions and TSRs import herbpy.action import herbpy.tsr # Setting necessary sim flags self.talker_simulated = talker_sim self.segway_sim = segway_sim # Set up perception self.detector = None if perception_sim: from prpy.perception import SimulatedPerceptionModule self.detector = SimulatedPerceptionModule() else: from prpy.perception import ApriltagsModule try: kinbody_path = FindCatkinResource('pr_ordata', 'data/objects') marker_data_path = FindCatkinResource( 'pr_ordata', 'data/objects/tag_data.json') self.detector = ApriltagsModule( marker_topic='/apriltags_kinect2/marker_array', marker_data_path=marker_data_path, kinbody_path=kinbody_path, detection_frame='head/kinect2_rgb_optical_frame', destination_frame='herb_base', reference_link=self.GetLink('/herb_base')) except IOError as e: logger.warning('Failed to find required resource path. ' \ 'pr_ordata package cannot be found. ' \ 'Perception detector will not be loaded.' \ '\n{}'.format(e)) if not self.talker_simulated: # Initialize herbpy ROS Node import rospy if not rospy.core.is_initialized(): raise RuntimeError('rospy not initialized. ' 'Must call rospy.init_node()') import talker.msg from actionlib import SimpleActionClient self._say_action_client = SimpleActionClient( 'say', talker.msg.SayAction)
def initialize(robot_xml=None, env_path=None, attach_viewer=False, sim=True, **kw_args): import prpy, os prpy.logger.initialize_logging() # Hide TrajOpt logging. os.environ.setdefault('TRAJOPT_LOG_THRESH', 'WARN') # Load plugins. if prpy.dependency_manager.is_catkin(): prpy.dependency_manager.export() else: prpy.dependency_manager.export(PACKAGE) openravepy.RaveInitialize(True) # Create the environment. env = openravepy.Environment() if env_path is not None: if not env.Load(env_path): raise Exception('Unable to load environment frompath %s' % env_path) if prpy.dependency_manager.is_catkin(): # Find the HERB URDF and SRDF files. from catkin.find_in_workspaces import find_in_workspaces share_directories = find_in_workspaces(search_dirs=['share'], project='herb_description') if not share_directories: logger.error('Unable to find the HERB model. Do you have the' ' package herb_description installed?') raise ValueError('Unable to find HERB model.') found_models = False for share_directory in share_directories: urdf_path = os.path.join(share_directories[0], 'robots', 'herb.urdf') srdf_path = os.path.join(share_directories[0], 'robots', 'herb.srdf') if os.path.exists(urdf_path) and os.path.exists(srdf_path): found_models = True break if not found_models: logger.error('Missing URDF file and/or SRDF file for HERB.' ' Is the herb_description package properly installed?') raise ValueError('Unable to find HERB URDF and SRDF files.') # Load the URDF file into OpenRAVE. urdf_module = openravepy.RaveCreateModule(env, 'urdf') if urdf_module is None: logger.error('Unable to load or_urdf module. Do you have or_urdf' ' built and installed in one of your Catkin workspaces?') raise ValueError('Unable to load or_urdf plugin.') args = 'Load {:s} {:s}'.format(urdf_path, srdf_path) herb_name = urdf_module.SendCommand(args) if herb_name is None: raise ValueError('Failed loading HERB model using or_urdf.') robot = env.GetRobot(herb_name) if robot is None: raise ValueError('Unable to find robot with name "{:s}".'.format( herb_name)) else: if robot_xml is None: import os, rospkg rospack = rospkg.RosPack() base_path = rospack.get_path('herb_description') robot_xml = os.path.join(base_path, 'ordata', 'robots', 'herb.robot.xml') robot = env.ReadRobotXMLFile(robot_xml) env.Add(robot) # Default arguments. keys = [ 'left_arm_sim', 'left_hand_sim', 'left_ft_sim', 'right_arm_sim', 'right_hand_sim', 'right_ft_sim', 'head_sim', 'talker_sim', 'segway_sim', 'perception_sim' ] for key in keys: if key not in kw_args: kw_args[key] = sim from herbrobot import HERBRobot prpy.bind_subclass(robot, HERBRobot, **kw_args) if sim: dof_indices, dof_values \ = robot.configurations.get_configuration('relaxed_home') robot.SetDOFValues(dof_values, dof_indices) # Start by attempting to load or_rviz. if attach_viewer == True: attach_viewer = 'rviz' env.SetViewer(attach_viewer) # Fall back on qtcoin if loading or_rviz failed if env.GetViewer() is None: logger.warning( 'Loading the RViz viewer failed. Do you have or_interactive' ' marker installed? Falling back on qtcoin.') attach_viewer = 'qtcoin' if attach_viewer and env.GetViewer() is None: env.SetViewer(attach_viewer) if env.GetViewer() is None: raise Exception('Failed creating viewer of type "{0:s}".'.format( attach_viewer)) # Remove the ROS logging handler again. It might have been added when we # loaded or_rviz. prpy.logger.remove_ros_logger() return env, robot
def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim, left_hand_sim, right_hand_sim, left_ft_sim, head_sim, talker_sim, segway_sim, perception_sim): from prpy.util import FindCatkinResource Robot.__init__(self, robot_name='herb') # Convenience attributes for accessing self components. self.left_arm = self.GetManipulator('left') self.right_arm = self.GetManipulator('right') self.head = self.GetManipulator('head') self.left_arm.hand = self.left_arm.GetEndEffector() self.right_arm.hand = self.right_arm.GetEndEffector() self.left_hand = self.left_arm.hand self.right_hand = self.right_arm.hand self.manipulators = [ self.left_arm, self.right_arm, self.head ] # Dynamically switch to self-specific subclasses. prpy.bind_subclass(self.left_arm, WAM, sim=left_arm_sim, owd_namespace='/left/owd') prpy.bind_subclass(self.right_arm, WAM, sim=right_arm_sim, owd_namespace='/right/owd') prpy.bind_subclass(self.head, HERBPantilt, sim=head_sim, owd_namespace='/head/owd') prpy.bind_subclass(self.left_arm.hand, BarrettHand, sim=left_hand_sim, manipulator=self.left_arm, owd_namespace='/left/owd', bhd_namespace='/left/bhd', ft_sim=right_ft_sim) prpy.bind_subclass(self.right_arm.hand, BarrettHand, sim=right_hand_sim, manipulator=self.right_arm, owd_namespace='/right/owd', bhd_namespace='/right/bhd', ft_sim=right_ft_sim) self.base = HerbBase(sim=segway_sim, robot=self) # Set HERB's acceleration limits. These are not specified in URDF. accel_limits = self.GetDOFAccelerationLimits() accel_limits[self.head.GetArmIndices()] = [ 2. ] * self.head.GetArmDOF() accel_limits[self.left_arm.GetArmIndices()] = [ 2. ] * self.left_arm.GetArmDOF() accel_limits[self.right_arm.GetArmIndices()] = [ 2. ] * self.right_arm.GetArmDOF() self.SetDOFAccelerationLimits(accel_limits) # Support for named configurations. import os.path self.configurations.add_group('left_arm', self.left_arm.GetArmIndices()) self.configurations.add_group('right_arm', self.right_arm.GetArmIndices()) self.configurations.add_group('head', self.head.GetArmIndices()) self.configurations.add_group('left_hand', self.left_hand.GetIndices()) self.configurations.add_group('right_hand', self.right_hand.GetIndices()) configurations_path = FindCatkinResource('herbpy', 'config/configurations.yaml') try: self.configurations.load_yaml(configurations_path) except IOError as e: raise ValueError('Failed laoding named configurations from "{:s}".'.format( configurations_path)) # Initialize a default planning pipeline. from prpy.planning import ( FirstSupported, MethodMask, Ranked, Sequence, ) from prpy.planning import ( CBiRRTPlanner, CHOMPPlanner, GreedyIKPlanner, IKPlanner, NamedPlanner, SBPLPlanner, SnapPlanner, TSRPlanner, VectorFieldPlanner ) # TODO: These should be meta-planners. self.named_planner = NamedPlanner() self.ik_planner = IKPlanner() # Special-purpose planners. self.snap_planner = SnapPlanner() self.vectorfield_planner = VectorFieldPlanner() self.greedyik_planner = GreedyIKPlanner() # General-purpose planners. self.cbirrt_planner = CBiRRTPlanner() # Trajectory optimizer. try: from or_trajopt import TrajoptPlanner self.trajopt_planner = TrajoptPlanner() except ImportError: self.trajopt_planner = None logger.warning('Failed creating TrajoptPlanner. Is the or_trajopt' ' package in your workspace and built?') try: self.chomp_planner = CHOMPPlanner() except UnsupportedPlanningError: self.chomp_planner = None logger.warning('Failed loading the CHOMP module. Is the or_cdchomp' ' package in your workspace and built?') if not (self.trajopt_planner or self.chomp_planner): raise PrPyException('Unable to load both CHOMP and TrajOpt. At' ' least one of these packages is required.') actual_planner = Sequence( # First, try the straight-line trajectory. self.snap_planner, # Then, try a few simple (and fast!) heuristics. self.vectorfield_planner, #self.greedyik_planner, # Next, try a trajectory optimizer. self.trajopt_planner or self.chomp_planner ) self.planner = FirstSupported( Sequence(actual_planner, TSRPlanner(delegate_planner=actual_planner), self.cbirrt_planner), # Special purpose meta-planner. NamedPlanner(delegate_planner=actual_planner), ) from prpy.planning.retimer import HauserParabolicSmoother self.smoother = HauserParabolicSmoother(do_blend=True, do_shortcut=True) self.retimer = HauserParabolicSmoother(do_blend=True, do_shortcut=False, blend_iterations=5, blend_radius=0.4) self.simplifier = None # Base planning from prpy.util import FindCatkinResource planner_parameters_path = FindCatkinResource('herbpy', 'config/base_planner_parameters.yaml') self.sbpl_planner = SBPLPlanner() try: with open(planner_parameters_path, 'rb') as config_file: import yaml params_yaml = yaml.load(config_file) self.sbpl_planner.SetPlannerParameters(params_yaml) except IOError as e: raise ValueError('Failed loading base planner parameters from "{:s}".'.format( planner_parameters_path)) self.base_planner = self.sbpl_planner # Create action library from prpy.action import ActionLibrary self.actions = ActionLibrary() # Register default actions and TSRs import herbpy.action import herbpy.tsr # Setting necessary sim flags self.talker_simulated = talker_sim self.segway_sim = segway_sim # Set up perception self.detector=None if perception_sim: from prpy.perception import SimulatedPerceptionModule self.detector = SimulatedPerceptionModule() else: from prpy.perception import ApriltagsModule try: kinbody_path = prpy.util.FindCatkinResource('pr_ordata', 'data/objects') marker_data_path = prpy.util.FindCatkinResource('pr_ordata', 'data/objects/tag_data.json') self.detector = ApriltagsModule(marker_topic='/apriltags_kinect2/marker_array', marker_data_path=marker_data_path, kinbody_path=kinbody_path, detection_frame='head/kinect2_rgb_optical_frame', destination_frame='map') except IOError as e: logger.warning('Failed to find required resource path. ' \ 'pr-ordata package cannot be found. ' \ 'Perception detector will not be loaded.') if not self.talker_simulated: # Initialize herbpy ROS Node import rospy if not rospy.core.is_initialized(): rospy.init_node('herbpy', anonymous=True) logger.debug('Started ROS node with name "%s".', rospy.get_name()) import talker.msg from actionlib import SimpleActionClient self._say_action_client = SimpleActionClient('say', talker.msg.SayAction)
def __init__(self, sim): """ Create an ADARobot. @param sim: Set to True to run the robot in simulation. """ from mico import Mico from micohand import MicoHand from util import AdaPyException, find_adapy_resource self.simulated = sim self.talker_simulated = sim # We need to hard-code the name. Otherwise, it defaults to # "mico-modified". Robot.__init__(self, robot_name='ada') # Convenience attributes for accessing self components. self.arm = self.GetManipulator('Mico') self.arm.hand = self.arm.GetEndEffector() self.manipulators = [ self.arm ] # Bind robot-specific subclasses. prpy.bind_subclass(self.arm, Mico, sim=sim) prpy.bind_subclass(self.arm.hand, MicoHand, sim=sim, manipulator=self.arm) # TODO: Load an IdealController controller in simulation. # Load default named configurations from YAML. self.configurations.add_group('arm', self.arm.GetIndices()) try: self.configurations.load_yaml( find_adapy_resource(CONFIGURATIONS_PATH)) except IOError as e: logger.warning('Failed loading named configurations from "%s": %s', configurations_path, e.message) # If in sim, set the robot DOFs to not be in collision if sim: inds,dofs = self.configurations.get_configuration('home') self.SetDOFValues(values=dofs,dofindices=inds) # Load default TSRs from YAML. for tsr_path_relative in TSR_PATHS: try: tsr_path = find_adapy_resource(tsr_path_relative) self.tsrlibrary.load_yaml(tsr_path) except IOError as e: raise ValueError( 'Failed loading TSRs from "{:s}": {:s}'.format( tsr_path, e.message)) # Create ros_control clients for execution on real hardware. if not sim: from .controller_client import (ControllerManagerClient, JointStateClient) from .trajectory_client import FollowJointTrajectoryClient self._trajectory_joint_names = [ self.GetJointFromDOFIndex(i).GetName() for i in xrange(self.GetDOF()) ] self._jointstate_client = JointStateClient( self, topic_name='/joint_states' ) self._controller_client = ControllerManagerClient( ns='/controller_manager' ) self._velocity_joint_mode_controller = self._controller_client.request( 'velocity_joint_mode_controller' ) self._velocity_joint_mode_controller.switch() self._trajectory_switcher = self._controller_client.request( 'traj_controller' ) self._trajectory_client = FollowJointTrajectoryClient( ns='/traj_controller/follow_joint_trajectory' ) # Initialize the default planning pipeline. from prpy.planning import Sequence, Ranked, FirstSupported from prpy.planning import ( BiRRTPlanner, CBiRRTPlanner, IKPlanner, GreedyIKPlanner, NamedPlanner, SBPLPlanner, SnapPlanner, IKPlanner, TSRPlanner, VectorFieldPlanner ) self.snap_planner = SnapPlanner() self.greedyik_planner = GreedyIKPlanner() self.cbirrt_planner = CBiRRTPlanner() self.vectorfield_planner = VectorFieldPlanner() # Hide TrajOpt logging. os.environ.setdefault('TRAJOPT_LOG_THRESH', 'WARN') # Trajectory optimizer. try: from or_trajopt import TrajoptPlanner self.trajopt_planner = TrajoptPlanner() except ImportError: self.trajopt_planner = None logger.warning('Failed creating TrajoptPlanner. Is the or_trajopt' ' package in your workspace and built?') ik_planners = Sequence( self.snap_planner, self.trajopt_planner, ) planner_for_ik = FirstSupported( ik_planners, NamedPlanner(delegate_planner=ik_planners) ) self.ik_planner = IKPlanner(delegate_planner=planner_for_ik) actual_planner = Sequence( self.snap_planner, self.ik_planner, self.trajopt_planner, self.vectorfield_planner, #self.greedyik_planner, self.cbirrt_planner ) self.planner = FirstSupported( actual_planner, NamedPlanner(delegate_planner=actual_planner) ) self.simplifier = None #from prpy.planning.retimer import ParabolicSmoother, HauserParabolicSmoother # self.smoother = Sequence( # ParabolicSmoother(), # HauserParabolicSmoother() # ) from prpy.planning.retimer import HauserParabolicSmoother self.smoother = HauserParabolicSmoother(do_blend=True, do_shortcut=True) self.retimer = HauserParabolicSmoother(do_blend=True, do_shortcut=False) from prpy.action import ActionLibrary self.actions = ActionLibrary() if not self.talker_simulated: # Initialize herbpy ROS Node import rospy if not rospy.core.is_initialized(): rospy.init_node('adapy', anonymous=True) logger.debug('Started ROS node with name "%s".', rospy.get_name())
def __init__(self, arm_sim, arm_torso_sim, gripper_sim, head_sim, torso_sim, base_sim, talker_sim, whole_body_sim, perception_sim, robot_checker_factory): Robot.__init__(self, robot_name='fetch') self.robot_checker_factory = robot_checker_factory #Controller Setup self.controller_manager = None self.controller_always_on = [] self.full_controller_sim = (arm_sim and arm_torso_sim and gripper_sim and head_sim) if not self.full_controller_sim: import rospy from ros_control_client_py import ( ControllerManagerClient, JointStateClient, ) import rosgraph.masterapi if not rospy.core.is_initialized(): raise RuntimeError('rospy not initialized.' 'Must call rospy.init_node()') master = rosgraph.masterapi.Master('/rostopic') topics = [] for i in master.getPublishedTopics(''): topics.append(i[0]) if '/joint_states' not in topics: raise RuntimeError( 'The -sim argument should be set if there is' ' No real robot/Gazebo robot connected') #update openrave state from /joint_states self._jointstate_client = JointStateClient( self, topic_name='/joint_states') self.controller_always_on.append('joint_state_controller') # Convenience attributes for accessing self components. self.arm = self.GetManipulator('arm') self.arm_torso = self.GetManipulator('arm_torso') self.gripper = self.arm.GetEndEffector() self.hand = self.gripper self.head = self.GetManipulator('head') self.manipulators = [self.arm, self.arm_torso] #Dynamically switch to self-specific subclasses. prpy.bind_subclass(self.arm, ARM, sim=arm_sim, namespace='arm') prpy.bind_subclass(self.arm_torso, ARM, sim=arm_torso_sim, namespace='arm_torso') prpy.bind_subclass(self.gripper, GRIPPER, sim=gripper_sim, manipulator=self.arm, namespace='') prpy.bind_subclass(self.head, HEAD, robot=self, sim=head_sim, namespace='') self.base = BASE(sim=base_sim, robot=self) self.whole_body = WholeBody(sim=whole_body_sim, robot=self) # Set FETCH's acceleration limits. These are not specified in URDF. accel_limits = self.GetDOFAccelerationLimits() accel_limits[self.arm.GetArmIndices()] = [2.] * self.arm.GetArmDOF() accel_limits[self.arm_torso.GetArmIndices( )] = [2.] * self.arm_torso.GetArmDOF() #get acceleration limits and set them here(change) self.SetDOFAccelerationLimits(accel_limits) # Determine always-on controllers # Set default manipulator controllers in sim only (change) if not gripper_sim: self.controller_always_on.append('gripper_controller') if not head_sim: self.controller_always_on.append('head_controller') if not base_sim: self.controller_always_on.append('base_controller') if arm_sim: self.arm.sim_controller = self.AttachController( name=self.arm.GetName(), args='IdealController', dof_indices=self.arm.GetArmIndices(), affine_dofs=0, simulated=True) # For simulation we cannot assign same DOF indices to different controllers # So for arm_torso only the torso is included if arm_torso_sim: self.arm_torso.sim_controller = self.AttachController( name=self.arm_torso.GetName(), args='IdealController', dof_indices=[11], affine_dofs=0, simulated=True) # Support for named configurations.(change) import os.path self.configurations.add_group('arm', self.arm.GetArmIndices()) #self.configurations.add_group('arm_torso',self.arm.GetArmIndices()) self.configurations.add_group('gripper', self.gripper.GetIndices()) self.configurations.add_group('head', self.head.GetIndices()) configurations_path = FindCatkinResource('fetchpy', 'config/configurations.yaml') try: self.configurations.load_yaml(configurations_path) except IOError as e: raise ValueError( 'Failed loading named configurations from "{:s}".'.format( configurations_path)) #Gripper configurations self.gripper.configurations = ConfigurationLibrary() self.gripper.configurations.add_group('gripper', self.gripper.GetIndices()) if isinstance(self.hand, GRIPPER): gripper_configurations_path = FindCatkinResource( 'fetchpy', 'config/gripper_preshapes.yaml') try: self.configurations.load_yaml(gripper_configurations_path) except IOError as e: raise ValueError( 'Failed loading named configurations from "{:s}".'.format( gripper_configurations_path)) else: logger.warning( 'Unrecognized hand class. Not loading named configurations.') #Head configurations self.head.configurations = ConfigurationLibrary() self.head.configurations.add_group('head', self.head.GetIndices()) if isinstance(self.head, HEAD): head_configurations_path = FindCatkinResource( 'fetchpy', 'config/head_preshapes.yaml') try: self.configurations.load_yaml(head_configurations_path) except IOError as e: raise ValueError( 'Failed loading named configurations from "{:s}".'.format( head_configurations_path)) else: logger.warning( 'Unrecognized HEAD class. Not loading named configurations.') #Planner. snap_planner = SnapPlanner( robot_checker_factory=self.robot_checker_factory) vectorfield_planner = VectorFieldPlanner( robot_checker_factory=self.robot_checker_factory) trajopt_planner = TrajoptPlanner( robot_checker_factory=self.robot_checker_factory) rrt_planner = OMPLPlanner( 'RRTConnect', robot_checker_factory=self.robot_checker_factory) cbirrt_planner = CBiRRTPlanner( timelimit=1., robot_checker_factory=self.robot_checker_factory) actual_planner = Sequence( snap_planner, vectorfield_planner, trajopt_planner, TSRPlanner(delegate_planner=Sequence(snap_planner, trajopt_planner), robot_checker_factory=self.robot_checker_factory), FirstSupported(rrt_planner, cbirrt_planner), ) self.planner = FirstSupported( actual_planner, NamedPlanner(delegate_planner=actual_planner), ) # Post-processor. self.smoother = HauserParabolicSmoother(do_blend=True, blend_iterations=1, blend_radius=0.4, do_shortcut=True, timelimit=0.6) self.retimer = HauserParabolicSmoother(do_blend=True, blend_iterations=1, blend_radius=0.4, do_shortcut=False) self.simplifier = None #Base Planning self.sbpl_planner = SBPLPlanner() self.base_planner = self.sbpl_planner #Create action library(change) self.actions = ActionLibrary() # Register default actions and TSRs: TODO(change) import fetchpy.action #import fetch.tsr #Setting necessary sim flags self.talker_simulated = talker_sim self.base_sim = base_sim #Set up perception(change) self.detector = None if perception_sim: from prpy.perception import SimulatedPerceptionModule self.detector = SimulatedPerceptionModule() else: from prpy.perception import ApriltagsModule try: kinbody_path = FindCatkinResource('pr_ordata', 'data/objects') marker_data_path = FindCatkinResource( 'pr_ordata', 'data/objects/tag_data.json') self.detector = ApriltagsModule( marker_topic='/apriltags_kinect2/marker_array', marker_data_path=marker_data_path, kinbody_path=kinbody_path, detection_frame='head/kinect2_rgb_optical_frame', destination_frame='fetch_base', reference_link=self.GetLink('/base')) except IOError as e: logger.warning('Failed to find required resource path. ' \ 'pr_ordata package cannot be found. ' \ 'Perception detector will not be loaded.' \ '\n{}'.format(e)) #TALKER if not self.talker_simulated: # Initialize fetchpy ROS Node if not rospy.core.is_initialized(): raise RuntimeError('rospy not initialized. ' 'Must call rospy.init_node()') from sound_play.msg import SoundRequest from sound_play.libsoundplay import SoundClient self.soundhandle = SoundClient() self.voice = 'voice_kal_diphone' self.volume = 1.0
def initialize(robot_xml=None, env_path=None, attach_viewer=False, sim=True, **kw_args): import prpy, os prpy.logger.initialize_logging() # Hide TrajOpt logging. os.environ.setdefault('TRAJOPT_LOG_THRESH', 'WARN') # Load plugins. if prpy.dependency_manager.is_catkin(): prpy.dependency_manager.export() else: prpy.dependency_manager.export(PACKAGE) openravepy.RaveInitialize(True) # Create the environment. env = openravepy.Environment() if env_path is not None: if not env.Load(env_path): raise Exception('Unable to load environment frompath %s' % env_path) # Load the robot model_name = None share_directories = None if robot_xml is None: if prpy.dependency_manager.is_catkin(): from catkin.find_in_workspaces import find_in_workspaces share_directories = find_in_workspaces(search_dirs=['share'], project=PACKAGE) if not share_directories: logger.error('Unable to find share directory in catkin workspace.') raise ValueError('Unable to find share directory in catkin workspace.') import os.path model_name = 'barrettsegway.robot.xml' for share_directory in share_directories: xml_path = os.path.join(share_directory, 'robots', model_name) if os.path.exists(xml_path): robot_xml = xml_path if robot_xml is None: err_str = 'Unable to find robot model with name: %s in directories %s.' % (model_name, share_directories) logger.error(err_str) raise ValueError(err_str) # Load the robot model robot = env.ReadRobotXMLFile(robot_xml) env.Add(robot) # Default arguments. keys = [ 'arm_sim', 'hand_sim', 'ft_sim', 'segway_sim' ] for key in keys: if key not in kw_args: kw_args[key] = sim from wamrobot import WAMRobot prpy.bind_subclass(robot, WAMRobot, **kw_args) if sim: dof_indices, dof_values \ = robot.configurations.get_configuration('zero') robot.SetDOFValues(dof_values, dof_indices) # Start by attempting to load or_rviz. if attach_viewer == True: attach_viewer = 'rviz' env.SetViewer(attach_viewer) # Fall back on qtcoin if loading or_rviz failed if env.GetViewer() is None: logger.warning( 'Loading the RViz viewer failed. Do you have or_interactive' ' marker installed? Falling back on qtcoin.') attach_viewer = 'qtcoin' if attach_viewer and env.GetViewer() is None: env.SetViewer(attach_viewer) if env.GetViewer() is None: raise Exception('Failed creating viewer of type "{0:s}".'.format( attach_viewer)) # Remove the ROS logging handler again. # It might have been added when we loaded or_rviz. prpy.logger.remove_ros_logger() return env, robot