Ejemplo n.º 1
0
 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)
Ejemplo n.º 2
0
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
Ejemplo n.º 3
0
    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())
Ejemplo n.º 4
0
    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)
Ejemplo n.º 5
0
    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
Ejemplo n.º 6
0
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
Ejemplo n.º 7
0
    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)
Ejemplo n.º 8
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)

    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
Ejemplo n.º 9
0
    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)
Ejemplo n.º 10
0
    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())
Ejemplo n.º 11
0
    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
Ejemplo n.º 12
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