def run(self):
        move_thread = None
        if not is_moveit_running():
            rospy.loginfo("starting moveit")
            move_thread = MoveItThread()

        rospy.loginfo("Waiting for MoveIt...")
        self.client = MoveGroupInterface("arm_with_torso", "base_link")
        rospy.loginfo("...connected")

        # Padding does not work (especially for self collisions)
        # So we are adding a box above the base of the robot
        scene = PlanningSceneInterface("base_link")
        scene.addBox("keepout", 0.2, 0.5, 0.05, 0.15, 0.0, 0.375)

        joints = ["torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
                  "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
        pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
        while not rospy.is_shutdown():
            result = self.client.moveToJointPosition(joints,
                                                     pose,
                                                     0.0,
                                                     max_velocity_scaling_factor=0.5)
            if result and result.error_code.val == MoveItErrorCodes.SUCCESS:
                scene.removeCollisionObject("keepout")
                if move_thread:
                    move_thread.stop()

                # On success quit
                # Stopping the MoveIt thread works, however, the action client
                # does not get shut down, and future tucks will not work.
                # As a work-around, we die and roslaunch will respawn us.
                rospy.signal_shutdown("done")
                sys.exit(0)
                return
Ejemplo n.º 2
0
    def __init__(self):
        self.scene = PlanningSceneInterface("base_link")
        self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
        self.move_group = MoveGroupInterface("arm", "base_link")

        find_topic = "basic_grasping_perception/find_objects"
        rospy.loginfo("Waiting for %s..." % find_topic)
        self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
        self.find_client.wait_for_server()
Ejemplo n.º 3
0
    def __init__(self, tower_array, disk_array, config, debug=0):
        print "[INFO] Initialise World"
        self.DEBUG = debug
        # initialise arrays
        self.tower_array = tower_array
        self.disk_array = disk_array
        # configs
        self.max_gripper = config.get_max_gripper()
        self.disk_height = config.disk_height
        self.tower_positions = config.tower_positions

        self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME)
        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
Ejemplo n.º 4
0
    def __init__(self):
        self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"]

        self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction)
        rospy.loginfo("Waiting for gripper_controller...")
        self.client.wait_for_server()

        self.scene = PlanningSceneInterface("base_link")
        self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True)

        find_topic = "basic_grasping_perception/find_objects"
        rospy.loginfo("Waiting for %s..." % find_topic)
        self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
        self.find_client.wait_for_server()
Ejemplo n.º 5
0
 def clean_up(self, close=False):
     if close:
         # get the actual list after game
         self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME)
     for name in self.planning_scene_interface.getKnownCollisionObjects():
         if self.DEBUG is 1:
             print("[DEBUG] Removing object %s" % name)
         self.planning_scene_interface.removeCollisionObject(name, False)
     for name in self.planning_scene_interface.getKnownAttachedObjects():
         if self.DEBUG is 1:
             print("[DEBUG] Removing attached object %s" % name)
         self.planning_scene_interface.removeAttachedObject(name, False)
     if close:
         self.planning_scene_interface.waitForSync(5.0)
         pass
    def __init__(self):
        self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"]

        self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction)
        rospy.loginfo("Waiting for gripper_controller...")
        self.client.wait_for_server()
        rospy.loginfo("...connected.")

        self.scene = PlanningSceneInterface("base_link")
        self.attached_object_publisher = rospy.Publisher(
            "attached_collision_object", AttachedCollisionObject, queue_size=10
        )
        self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True)

        find_topic = "basic_grasping_perception/find_objects"
        rospy.loginfo("Waiting for %s..." % find_topic)
        self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
        self.find_client.wait_for_server()
        self.debug_index = -1

        self.graspables = []
Ejemplo n.º 7
0
    def run(self):
        move_thread = None
        if not is_moveit_running():
            rospy.loginfo("starting moveit")
            move_thread = MoveItThread()

        rospy.loginfo("Waiting for MoveIt...")
        self.client = MoveGroupInterface("arm_with_torso", "base_link")
        rospy.loginfo("...connected")

        # Padding does not work (especially for self collisions)
        # So we are adding a box above the base of the robot
        scene = PlanningSceneInterface("base_link")
        scene.addBox("keepout", 0.2, 0.5, 0.05, 0.15, 0.0, 0.375)

        joints = [
            "torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
            "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
            "wrist_flex_joint", "wrist_roll_joint"
        ]
        pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
        while not rospy.is_shutdown():
            result = self.client.moveToJointPosition(
                joints, pose, 0.0, max_velocity_scaling_factor=0.5)
            if result and result.error_code.val == MoveItErrorCodes.SUCCESS:
                scene.removeCollisionObject("keepout")
                if move_thread:
                    move_thread.stop()

                # On success quit
                # Stopping the MoveIt thread works, however, the action client
                # does not get shut down, and future tucks will not work.
                # As a work-around, we die and roslaunch will respawn us.
                rospy.signal_shutdown("done")
                sys.exit(0)
                return
Ejemplo n.º 8
0
class GripperInterfaceClient():
 
    def __init__(self):
        self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"]

        self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action",
                                                   GripperCommandAction)
        rospy.loginfo("Waiting for gripper_controller...")
        self.client.wait_for_server() 
    
        self.scene = PlanningSceneInterface("base_link")
        self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True)

        find_topic = "basic_grasping_perception/find_objects"
        rospy.loginfo("Waiting for %s..." % find_topic)
        self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
        self.find_client.wait_for_server()

    def execute_trajectory(self, trajectory):
        goal_position = trajectory.points[-1].positions
        print '==========', goal_position
        command_goal = GripperCommandGoal()
        command_goal.command.position = goal_position[0]+goal_position[1]
        command_goal.command.max_effort = 50.0 # Placeholder. TODO Figure out a better way to compute this
        self.client.send_goal(command_goal)
        self.client.wait_for_result()

    def move_to(self, goal_position, max_effort):
        command_goal = GripperCommandGoal()
        command_goal.command.position = goal_position
        command_goal.command.max_effort = max_effort
        self.client.send_goal(command_goal)
        self.client.wait_for_result()


    def updateScene(self):
        # find objects
        goal = FindGraspableObjectsGoal()
        goal.plan_grasps = True
        self.find_client.send_goal(goal)
        self.find_client.wait_for_result(rospy.Duration(5.0))
        find_result = self.find_client.get_result()

        # remove previous objects
        for name in self.scene.getKnownCollisionObjects():
            self.scene.removeCollisionObject(name, False)
        for name in self.scene.getKnownAttachedObjects():
            self.scene.removeAttachedObject(name, False)
        self.scene.waitForSync()

        # insert objects to scene
        idx = -1
        for obj in find_result.objects:
            idx += 1
            obj.object.name = "object%d"%idx
            self.scene.addSolidPrimitive(obj.object.name,
                                         obj.object.primitives[0],
                                         obj.object.primitive_poses[0],
                                         wait = False)
        # Try mesh representation
        #idx = -1
        #for obj in find_result.objects:
        #    idx += 1
        #    obj.object.name = "object%d"%idx
        #    self.scene.addMesh(obj.object.name,
        #                       obj.object.mesh_poses[0],
        #                       obj.object.meshes[0],
        #                       wait = False)

        for obj in find_result.support_surfaces:
            # extend surface to floor, and make wider since we have narrow field of view
            height = obj.primitive_poses[0].position.z
            obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0] + 0.1,
                                            2.5,  # wider
					    obj.primitives[0].dimensions[2] + height]
            obj.primitive_poses[0].position.z += -height/2.0

            # add to scene
            self.scene.addSolidPrimitive(obj.name,
                                         obj.primitives[0],
                                         obj.primitive_poses[0],
                                         wait = False)

        self.scene.waitForSync()

        # store for grasping
        self.objects = find_result.objects
        self.surfaces = find_result.support_surfaces

    def getGraspableCube(self):
        graspable = None
        for obj in self.objects:
            # need grasps
            if len(obj.grasps) < 1:
                continue
            # check size
            # if obj.object.primitives[0].dimensions[0] < 0.03 or \
            #   obj.object.primitives[0].dimensions[0] > 0.04 or \
            #   obj.object.primitives[0].dimensions[0] < 0.07 or \
            #   obj.object.primitives[0].dimensions[0] > 0.09 or \
            #   obj.object.primitives[2].dimensions[2] < 0.19 or \
            #   obj.object.primitives[2].dimensions[2] > 0.20:
            #    continue
            # has to be on table
            #if obj.object.primitive_poses[0].position.z < 0.5:
            #    continue
            return obj.object, obj.grasps
        # nothing detected
        return None, None

    def plan_pick(self, block, grasps):
        success, pick_result = self.pickplace.pick_with_retry(block.name,
                                                              grasps,
                                                              support_name=block.support_surface,
                                                              allow_gripper_support_collision=False,
                                                              planner_id = 'PRMkConfigDefault',
                                                              retries = 2,
                                                              scene=self.scene)
        #self.pick_result = pick_result
        if success:
           return pick_result.trajectory_stages
        else:
           rospy.loginfo("Planning Failed.")
           return None
Ejemplo n.º 9
0
# Note: fetch_moveit_config move_group.launch must be running
# Safety!: Do NOT run this script near people or objects.
# Safety!: There is NO perception.
#          The ONLY objects the collision detection software is aware
#          of are itself & the floor.
if __name__ == '__main__':
    rospy.init_node("simple_disco")

    # Create move group interface for a fetch robot
    move_group = MoveGroupInterface("arm_with_torso", "base_link")

    # Define ground plane
    # This creates objects in the planning scene that mimic the ground
    # If these were not in place gripper could hit the ground
    planning_scene = PlanningSceneInterface("base_link")
    planning_scene.removeCollisionObject("my_front_ground")
    planning_scene.removeCollisionObject("my_back_ground")
    planning_scene.removeCollisionObject("my_right_ground")
    planning_scene.removeCollisionObject("my_left_ground")
    planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
    planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
    planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
    planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)

    # TF joint names
    joint_names = [
        "torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
        "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
        "wrist_flex_joint", "wrist_roll_joint"
    ]
Ejemplo n.º 10
0
class FetchRobotApi:
    def __init__(self):

        self.moving_mode = False
        self.waldo_mode = False
        self.prev_joy_buttons = 0

        # See http://docs.fetchrobotics.com/robot_hardware.html#naming-conventions
        self.joint_names = [
            #'torso_lift_joint',
            #'bellows_joint',
            #'head_pan_joint',
            #'head_tilt_joint',
            'shoulder_pan_joint',
            'shoulder_lift_joint',
            'upperarm_roll_joint',
            'elbow_flex_joint',
            'forearm_roll_joint',
            'wrist_flex_joint',
            'wrist_roll_joint',
            'l_gripper_finger_joint',
            #'r_gripper_finger_joint',
        ]
        self.joint_name_map = dict([
            (name, index) for index, name in enumerate(self.joint_names)
        ])
        self.cur_joint_pos = np.zeros([len(self.joint_names)])
        self.cur_joint_vel = np.zeros([len(self.joint_names)])
        self.cur_joint_effort = np.zeros([len(self.joint_names)])

        self.cur_base_scan = np.zeros([662], dtype=np.float32)
        self.cur_head_camera_depth_image = np.zeros([480, 640],
                                                    dtype=np.float32)
        self.cur_head_camera_rgb_image = np.zeros([480, 640, 3],
                                                  dtype=np.uint8)

        self.timeseq = None
        self.timeseq_mutex = threading.Lock()
        self.image_queue = Queue.Queue()
        self.image_compress_thread = threading.Thread(
            target=self.image_compress_main)
        self.image_compress_thread.daemon = True
        self.image_compress_thread.start()

        logger.info('Subscribing...')
        self.subs = []
        if 1:
            self.subs.append(
                rospy.Subscriber('/joint_states', JointState,
                                 self.joint_states_cb))
        if 0:
            self.subs.append(
                rospy.Subscriber('/base_scan', LaserScan, self.base_scan_cb))
        if 1:
            self.subs.append(
                rospy.Subscriber('/head_camera/depth/image', numpy_msg(Image),
                                 self.head_camera_depth_image_cb))
        if 1:
            self.subs.append(
                rospy.Subscriber('/head_camera/rgb/image_raw',
                                 numpy_msg(Image),
                                 self.head_camera_rgb_image_raw_cb))
        if 1:
            self.subs.append(
                rospy.Subscriber('/spacenav/joy', Joy, self.spacenav_joy_cb))
        logger.info('Subscribed')

        self.arm_effort_pub = rospy.Publisher(
            '/arm_controller/weightless_torque/command',
            JointTrajectory,
            queue_size=2)
        #self.head_goal_pub = rospy.Publisher('/head_controller/point_head/goal', PointHeadActionGoal, queue_size=2)
        self.gripper_client = actionlib.SimpleActionClient(
            'gripper_controller/gripper_action', GripperCommandAction)

        self.arm_cartesian_twist_pub = rospy.Publisher(
            '/arm_controller/cartesian_twist/command', Twist, queue_size=2)

        self.head_point_client = actionlib.SimpleActionClient(
            'head_controller/point_head', PointHeadAction)

        self.arm_move_group = MoveGroupInterface("arm",
                                                 "base_link",
                                                 plan_only=True)
        self.arm_trajectory_client = actionlib.SimpleActionClient(
            "arm_controller/follow_joint_trajectory",
            FollowJointTrajectoryAction)
        self.arm_trajectory_client.wait_for_server()

        if 0:
            logger.info('Creating MoveGroupInterface...')
            self.move_group = MoveGroupInterface('arm_with_torso',
                                                 'base_link',
                                                 plan_only=True)
            logger.info('Created MoveGroupInterface')
            if 0:
                logger.info('Creating PlanningSceneInterface...')
                self.planning_scene = PlanningSceneInterface('base_link')
                self.planning_scene.removeCollisionObject('my_front_ground')
                self.planning_scene.removeCollisionObject('my_back_ground')
                self.planning_scene.removeCollisionObject('my_right_ground')
                self.planning_scene.removeCollisionObject('my_left_ground')
                self.planning_scene.addCube('my_front_ground', 2, 1.1, 0.0,
                                            -1.0)
                self.planning_scene.addCube('my_back_ground', 2, -1.2, 0.0,
                                            -1.0)
                self.planning_scene.addCube('my_left_ground', 2, 0.0, 1.2,
                                            -1.0)
                self.planning_scene.addCube('my_right_ground', 2, 0.0, -1.2,
                                            -1.0)

        logger.warn('FetchRobotGymEnv ROS node running')

        self.head_point_client.wait_for_server()
        logger.warn('FetchRobotGymEnv ROS node connected')

    def start_timeseq(self, script):
        timeseq = TimeseqWriter.create(script)
        timeseq.add_channel('robot_joints', 'FetchRobotJoints')
        timeseq.add_channel('gripper_joints', 'FetchGripperJoints')

        timeseq.add_schema(
            'FetchRobotJoints',
            ('torso_lift_joint', 'JointState'),
            ('head_pan_joint', 'JointState'),
            ('head_tilt_joint', 'JointState'),
            ('shoulder_pan_joint', 'JointState'),
            ('shoulder_lift_joint', 'JointState'),
            ('upperarm_roll_joint', 'JointState'),
            ('elbow_flex_joint', 'JointState'),
            ('forearm_roll_joint', 'JointState'),
            ('wrist_flex_joint', 'JointState'),
            ('wrist_roll_joint', 'JointState'),
        )
        timeseq.add_schema(
            'FetchGripperJoints',
            ('l_gripper_finger_joint', 'JointState'),
        )
        timeseq.add_schema(
            'JointState',
            ('pos', 'double'),
            ('vel', 'double'),
            ('effort', 'double'),
        )
        timeseq.add_channel('arm_action', 'FetchArmWeightlessTorqueAction')
        timeseq.add_schema(
            'FetchArmWeightlessTorqueAction',
            ('shoulder_pan_joint', 'WeightlessTorqueAction'),
            ('shoulder_lift_joint', 'WeightlessTorqueAction'),
            ('upperarm_roll_joint', 'WeightlessTorqueAction'),
            ('elbow_flex_joint', 'WeightlessTorqueAction'),
            ('forearm_roll_joint', 'WeightlessTorqueAction'),
            ('wrist_flex_joint', 'WeightlessTorqueAction'),
            ('wrist_roll_joint', 'WeightlessTorqueAction'),
        )
        timeseq.add_schema(
            'WeightlessTorqueAction',
            ('action', 'double'),
            ('effort', 'double'),
        )

        timeseq.add_channel('gripper_action', 'FetchGripperAction')
        timeseq.add_schema(
            'FetchGripperAction',
            ('action', 'double'),
            ('effort', 'double'),
            ('pos', 'double'),
        )

        timeseq.add_channel('head_camera_rgb', 'VideoFrame')
        timeseq.add_schema(
            'VideoFrame',
            ('url', 'string'),
        )
        with self.timeseq_mutex:
            if self.timeseq:
                self.timeseq.close()
            self.timeseq = timeseq

    def close_timeseq(self):
        with self.timeseq_mutex:
            if self.timeseq is not None:
                self.timeseq.close()
                threading.Thread(target=self.timeseq.upload_s3).start()
                self.timeseq = None

    def start_axis_video(self):
        cameras = rospy.get_param('/axis_cameras')
        if cameras and len(cameras):
            with self.timeseq_mutex:
                if self.timeseq:
                    for name, info in cameras.items():
                        logging.info('Camera %s: %s', name, repr(info))
                        self.timeseq.start_axis_video(
                            timeseq_name=name,
                            auth_header=info.get('auth_header'),
                            daemon_endpoint=info.get('daemon_endpoint'),
                            ipaddr=info.get('ipaddr'),
                            local_link_prefix=info.get('local_link_prefix'),
                            remote_traces_dir=info.get('remote_traces_dir'),
                            resolution=info.get('resolution'))

    def stop_axis_video(self):
        with self.timeseq_mutex:
            if self.timeseq:
                self.timeseq.stop_axis_video()

    def base_scan_cb(self, data):
        # fmin replaces nans with 15
        self.cur_base_scan = np.fmin(np.array(data.ranges), 15.0)

    def head_camera_depth_image_cb(self, data):
        shape = [data.height, data.width]
        dtype = np.dtype(np.float32)
        npdata = np.fromstring(data.data, dtype=dtype).reshape(shape)
        npdata.strides = (data.step, dtype.itemsize)
        self.cur_head_camera_depth_image = np.fmin(npdata, 5.0)

    def head_camera_rgb_image_raw_cb(self, data):
        shape = [data.height, data.width, 3]
        dtype = np.dtype(np.uint8)
        npdata = np.fromstring(data.data, dtype=dtype).reshape(shape)
        npdata.strides = (data.step, dtype.itemsize * 3, 1)
        self.cur_head_camera_rgb_image = npdata

        if self.timeseq:
            self.image_queue.put(('head_camera_rgb', data))

    def joint_states_cb(self, data):
        """
        Handle a joint_states message. Messages can have subsets of the joints, hopefully non-overlapping.
        """
        t0 = time.time()
        for i in range(len(data.name)):
            name = data.name[i]
            jni = self.joint_name_map.get(name, -1)
            if jni >= 0:
                self.cur_joint_pos[jni] = data.position[i]
                self.cur_joint_vel[jni] = data.velocity[i]
                self.cur_joint_effort[jni] = data.effort[i]

        with self.timeseq_mutex:
            if self.timeseq:
                channel, channel_type = (
                    (data.name[0] == 'l_wheel_joint' and
                     ('robot_joints', 'FetchRobotJoints'))
                    or (data.name[0] == 'l_gripper_finger_joint' and
                        ('gripper_joints', 'FetchGripperJoints'))
                    or (None, None))
                if channel:
                    state = dict([(jn, {
                        '__type': 'JointState',
                        'pos': data.position[i],
                        'vel': data.velocity[i],
                        'effort': data.effort[i],
                    }) for i, jn in enumerate(data.name)])
                    state['__type'] = channel_type
                    state['rxtime'] = t0
                    self.timeseq.add(data.header.stamp.to_sec(), channel,
                                     state)
                if 0:
                    t1 = time.time()
                    print '%0.6f+%0.6f stamp=%0.6f (age %0.6f) from %s' % (
                        t0, t1 - t0, data.header.stamp.to_sec(),
                        data.header.stamp.to_sec() - t0, channel)

    def image_compress_main(self):
        while True:
            channel, data = self.image_queue.get()
            if 0:
                print 'len(data.data)=%d data.width=%d data.height=%d' % (len(
                    data.data), data.width, data.height)
            im = PIL.Image.frombytes('RGB', (data.width, data.height),
                                     data.data, 'raw')
            jpeg_writer = StringIO()
            im.save(jpeg_writer, 'jpeg')
            im_url = 'data:image/jpeg;base64,' + base64.b64encode(
                jpeg_writer.getvalue())
            with self.timeseq_mutex:
                if self.timeseq:
                    self.timeseq.add(data.header.stamp.to_sec(), channel, {
                        '__type': 'VideoFrame',
                        'url': im_url,
                    })

    def move_to_start(self):

        self.moving_mode = True
        # Look down
        goal = PointHeadGoal()
        goal.target.header.stamp = rospy.Time.now()
        goal.target.header.frame_id = '/base_link'
        goal.target.point.x = 1.5
        goal.target.point.y = 0.0
        goal.target.point.z = -0.2
        goal.min_duration = rospy.Duration(0.5)
        if 0: logger.info('Point head to %s...', goal)
        self.head_point_client.send_goal(goal)
        if 0: logger.info('Point head sent')

        goal = GripperCommandGoal()
        goal.command.max_effort = 60
        goal.command.position = 0.1
        self.gripper_client.send_goal(goal)

        joints = [
            'shoulder_pan_joint',
            'shoulder_lift_joint',
            'upperarm_roll_joint',
            'elbow_flex_joint',
            'forearm_roll_joint',
            'wrist_flex_joint',
            'wrist_roll_joint',
        ]
        pose = [0.0, +0.8, 0.0, -0.8, 0.0, 0.0, 0.0]
        result = self.arm_move_group.moveToJointPosition(joints,
                                                         pose,
                                                         plan_only=True)
        if result.error_code.val == MoveItErrorCodes.SUCCESS:
            if 0: logger.info('Got trajectory %s', result.planned_trajectory)
            follow_goal = FollowJointTrajectoryGoal()
            follow_goal.trajectory = result.planned_trajectory.joint_trajectory
            logger.info('sending trajectory to arm...')
            self.arm_trajectory_client.send_goal(follow_goal)
            result = self.arm_trajectory_client.wait_for_result()
            logger.info('arm followed trajectory %s', result)
        else:
            logger.warn('moveToJointPosition returned %s', result)
            return

        result = self.head_point_client.wait_for_result()
        logger.info('head followed trajectory %s', result)

        logger.info('sending empty arm goal')
        empty_goal = FollowJointTrajectoryGoal()
        self.arm_trajectory_client.send_goal(empty_goal)

        logger.info('Point head done')
        self.moving_mode = False

    def set_arm_action(self, action):
        arm_joints = [
            ('shoulder_pan_joint', 1.57, 33.82),
            ('shoulder_lift_joint', 1.57, 131.76),
            ('upperarm_roll_joint', 1.57, 76.94),
            ('elbow_flex_joint', 1.57, 66.18),
            ('forearm_roll_joint', 1.57, 29.35),
            ('wrist_flex_joint', 2.26, 25.70),
            ('wrist_roll_joint', 2.26, 7.36),
        ]
        arm_efforts = [
            min(1.0, max(-1.0, action[self.joint_name_map.get(name)])) *
            torque_scale * 0.75 for name, vel_scale, torque_scale in arm_joints
        ]
        arm_joint_names = [
            name for name, vel_scale, torque_scale in arm_joints
        ]
        if 1:
            arm_joint_names.append('gravity_compensation')
            arm_efforts.append(1.0)
        arm_msg = JointTrajectory(
            joint_names=arm_joint_names,
            points=[JointTrajectoryPoint(effort=arm_efforts)])
        self.arm_effort_pub.publish(arm_msg)

        with self.timeseq_mutex:
            if self.timeseq:
                state = dict([(jn, {
                    '__type': 'WeightlessTorqueAction',
                    'action': action[self.joint_name_map.get(jn)],
                    'effort': arm_efforts[i],
                }) for i, jn in enumerate(arm_joint_names)])
                state['__type'] = 'FetchArmWeightlessTorqueAction'
                self.timeseq.add(time.time(), 'arm_action', state)

    def set_gripper_action(self, action):

        grip = action[self.joint_name_map.get('l_gripper_finger_joint')]

        goal = GripperCommandGoal()
        if grip > 0:
            goal.command.max_effort = 60 * min(1.0, grip)
            goal.command.position = 0.0
        else:
            goal.command.max_effort = 60
            goal.command.position = 0.1

        self.gripper_client.send_goal(goal)

        with self.timeseq_mutex:
            if self.timeseq:
                state = {
                    '__type': 'FetchGripperAction',
                    'action': grip,
                    'effort': goal.command.max_effort,
                    'pos': goal.command.position,
                }
                self.timeseq.add(time.time(), 'gripper_action', state)

    def set_waldo_action(self, joy):
        twist = Twist()
        twist.linear.x = joy.axes[0]
        twist.linear.y = joy.axes[1]
        twist.linear.z = joy.axes[2]
        twist.angular.x = +3.0 * joy.axes[3]
        twist.angular.y = +3.0 * joy.axes[4]
        twist.angular.z = +2.0 * joy.axes[5]
        self.arm_cartesian_twist_pub.publish(twist)

        if joy.buttons[1] and not self.prev_joy_buttons[1]:
            goal = GripperCommandGoal()
            goal.command.max_effort = 60
            goal.command.position = 0.0
            self.gripper_client.send_goal(goal)
        elif not joy.buttons[1] and self.prev_joy_buttons[1]:
            goal = GripperCommandGoal()
            goal.command.max_effort = 60
            goal.command.position = 0.1
            self.gripper_client.send_goal(goal)

    def spacenav_joy_cb(self, joy):
        if 0: logger.info('joy %s', str(joy))
        if joy.buttons[0] and not self.prev_joy_buttons[0]:
            if self.waldo_mode:
                self.stop_waldo_mode()
            elif not self.moving_mode:
                self.start_waldo_mode()

        if self.waldo_mode and not self.moving_mode:
            self.set_waldo_action(joy)
        self.prev_joy_buttons = joy.buttons

    def start_waldo_mode(self):
        logger.info('Start waldo mode')
        self.waldo_mode = True
        self.start_timeseq('fetchwaldo')
        self.start_axis_video()

    def stop_waldo_mode(self):
        logger.info('Stop waldo mode')
        self.waldo_mode = False
        timeseq_url = None
        if self.timeseq:
            timeseq_url = self.timeseq.get_url()
        logger.info('save_logs url=%s', timeseq_url)
        self.stop_axis_video()
        self.close_timeseq()

        mts_thread = threading.Thread(target=self.move_to_start)
        mts_thread.daemon = True
        mts_thread.start()
Ejemplo n.º 11
0
def picknplace():
    # Define initial parameters
    p = PlanningSceneInterface("base")
    g = MoveGroupInterface("both_arms", "base")
    gr = MoveGroupInterface("right_arm", "base")
    gl = MoveGroupInterface("left_arm", "base")
    leftgripper = baxter_interface.Gripper('left')
    leftgripper.calibrate()
    leftgripper.open()
    # jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
    # jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
    # jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2']
    # pos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
    # pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
    # lpos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519]
    # lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384]
    # rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
    # rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]

    # Clear planning scene
    # p.clear()
    # # Add table as attached object
    # p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])

    # Move both arms to start state
    # g.moveToJointPosition(jts_both, pos1, plan_only=False)

    # # Get obj locations
    # temp = rospy.wait_for_message("Dpos", PoseArray)
    # locs = temp.poses
    # locs_x = []
    # locs_y = []
    # orien = []
    # size = []

    # for i in range(len(locs)):
    #     locs_x.append(0.57 + locs[i].position.x)
    #     locs_y.append(-0.011 + locs[i].position.y)
    #     orien.append(locs[i].position.z*pi/180)
    #     size.append(locs[i].orientation.x)

    # # Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
    # ind_rmv = []
    # for i in range(0,len(locs)):
    #     if (locs_y[i] > 0.42):
    #         ind_rmv.append(i)
    #         continue
    #     for j in range(i,len(locs)):
    #         if not (i == j):
    #             if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
    #                 ind_rmv.append(i)

    # locs_x = del_meth(locs_x, ind_rmv)
    # locs_y = del_meth(locs_y, ind_rmv)
    # orien = del_meth(orien, ind_rmv)
    # size = del_meth(size, ind_rmv)

    # # Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
    # if locs_x:
    #     ig0 = itemgetter(0)
    #     sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))

    #     locs_x = list(sorted_lists[1])
    #     locs_y = list(sorted_lists[2])
    #     orien = list(sorted_lists[3])
    #     size = list(sorted_lists[0])

    # # Loop to continue pick and place until all objects are cleared from table
    # j=0
    # k=0
    # while locs_x:
    #     p.clear() # CLear planning scene of all collision objects

    #     # Attach table as attached collision object to base of baxter
    #     p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])
    #     xn = locs_x[0]
    #     yn = locs_y[0]
    #     zn = -0.06
    #     thn = orien[0]
    #     sz = size[0]
    #     if thn > pi/4:
    #         thn = -1*(thn%(pi/4))

    #     # Add collision objects into planning scene
    #     objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']
    #     for i in range(1,len(locs_x)):
    #         p.addCube(objlist[i], 0.05, locs_x[i], locs_y[i], -0.05)
    #     p.waitForSync()

    #     # Move both arms to pos2 (Right arm away and left arm on table)
    #     g.moveToJointPosition(jts_both, pos2, plan_only=False)

    # Move left arm to pick object and pick object
    # pickgoal = PoseStamped()
    # pickgoal.header.frame_id = "base"
    # pickgoal.header.stamp = rospy.Time.now()
    # pickgoal.pose.position.x = xn
    # pickgoal.pose.position.y = yn
    # pickgoal.pose.position.z = zn+0.1
    # pickgoal.pose.orientation.x = 1.0
    # pickgoal.pose.orientation.y = 0.0
    # pickgoal.pose.orientation.z = 0.0
    # pickgoal.pose.orientation.w = 0.0

    goal_pose = PoseStamped()
    limb = baxter_interface.Limb('left')
    gripper_pose = limb.endpoint_pose()
    goal_pose.pose.position.x = gripper_pose['position'][0]  #0.63
    goal_pose.pose.position.y = gripper_pose['position'][1]  #0.19519
    goal_pose.pose.position.z = gripper_pose['position'][2] + 0.1  #0.05

    goal_pose.pose.orientation.x = gripper_pose['orientation'][0]
    goal_pose.pose.orientation.y = gripper_pose['orientation'][1]
    goal_pose.pose.orientation.z = gripper_pose['orientation'][2]
    goal_pose.pose.orientation.w = gripper_pose['orientation'][3]
    gl.moveToPose(goal_pose, "left_gripper", plan_only=False)
Ejemplo n.º 12
0
class GraspingClient(object):

    def __init__(self):
        self.scene = PlanningSceneInterface("base_link")
        self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
        self.move_group = MoveGroupInterface("arm", "base_link")

        find_topic = "basic_grasping_perception/find_objects"
        rospy.loginfo("Waiting for %s..." % find_topic)
        self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
        self.find_client.wait_for_server()

    def updateScene(self):
        # find objects
        goal = FindGraspableObjectsGoal()
        goal.plan_grasps = True
        self.find_client.send_goal(goal)
        self.find_client.wait_for_result(rospy.Duration(5.0))
        find_result = self.find_client.get_result()

        # remove previous objects
        for name in self.scene.getKnownCollisionObjects():
            self.scene.removeCollisionObject(name, False)
        for name in self.scene.getKnownAttachedObjects():
            self.scene.removeAttachedObject(name, False)
        self.scene.waitForSync()

        # insert objects to scene
        idx = -1
        for obj in find_result.objects:
            idx += 1
            obj.object.name = "object%d"%idx
            self.scene.addSolidPrimitive(obj.object.name,
                                         obj.object.primitives[0],
                                         obj.object.primitive_poses[0],
                                         wait = False)

        for obj in find_result.support_surfaces:
            # extend surface to floor, and make wider since we have narrow field of view
            height = obj.primitive_poses[0].position.z
            obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
                                            1.5,  # wider
                                            obj.primitives[0].dimensions[2] + height]
            obj.primitive_poses[0].position.z += -height/2.0

            # add to scene
            self.scene.addSolidPrimitive(obj.name,
                                         obj.primitives[0],
                                         obj.primitive_poses[0],
                                         wait = False)

        self.scene.waitForSync()

        # store for grasping
        self.objects = find_result.objects
        self.surfaces = find_result.support_surfaces

    def getGraspableCube(self):
        graspable = None
        for obj in self.objects:
            # need grasps
            if len(obj.grasps) < 1:
                continue
            # check size
            if obj.object.primitives[0].dimensions[0] < 0.05 or \
               obj.object.primitives[0].dimensions[0] > 0.07 or \
               obj.object.primitives[0].dimensions[0] < 0.05 or \
               obj.object.primitives[0].dimensions[0] > 0.07 or \
               obj.object.primitives[0].dimensions[0] < 0.05 or \
               obj.object.primitives[0].dimensions[0] > 0.07:
                continue
            # has to be on table
            if obj.object.primitive_poses[0].position.z < 0.5:
                continue
            return obj.object, obj.grasps
        # nothing detected
        return None, None

    def getSupportSurface(self, name):
        for surface in self.support_surfaces:
            if surface.name == name:
                return surface
        return None

    def getPlaceLocation(self):
        pass

    def pick(self, block, grasps):
        success, pick_result = self.pickplace.pick_with_retry(block.name,
                                                              grasps,
                                                              support_name=block.support_surface,
                                                              scene=self.scene)
        self.pick_result = pick_result
        return success

    def place(self, block, pose_stamped):
        places = list()
        l = PlaceLocation()
        l.place_pose.pose = pose_stamped.pose
        l.place_pose.header.frame_id = pose_stamped.header.frame_id

        # copy the posture, approach and retreat from the grasp used
        l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
        l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
        l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
        places.append(copy.deepcopy(l))
        # create another several places, rotate each by 360/m degrees in yaw direction
        m = 16 # number of possible place poses
        pi = 3.141592653589
        for i in range(0, m-1):
            l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m)
            places.append(copy.deepcopy(l))

        success, place_result = self.pickplace.place_with_retry(block.name,
                                                                places,
                                                                scene=self.scene)
        return success

    def tuck(self):
        joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
                  "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
        pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
        while not rospy.is_shutdown():
            result = self.move_group.moveToJointPosition(joints, pose, 0.02)
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                return
Ejemplo n.º 13
0
def main():
    rospy.init_node("arm_obstacle_demo")
    wait_for_time()
    argv = rospy.myargv()


    planning_scene = PlanningSceneInterface("base_link")

    # Create table obstacle
    planning_scene.removeCollisionObject('table')
    table_size_x = 0.5
    table_size_y = 1
    table_size_z = 0.03
    table_x = 0.8
    table_y = 0
    table_z = 0.6
    planning_scene.addBox('table', table_size_x, table_size_y, table_size_z,
                          table_x, table_y, table_z)

    # Create divider obstacle
    planning_scene.removeCollisionObject('divider')
    size_x = 0.3 
    size_y = 0.01
    size_z = 0.2
    x = table_x - (table_size_x / 2) + (size_x / 2)
    y = 0 
    z = table_z + (table_size_z / 2) + (size_z / 2)
    planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z)

    pose1 = PoseStamped()
    pose1.header.frame_id = 'base_link'
    pose1.pose.position.x = 0.5
    pose1.pose.position.y = -0.3
    pose1.pose.position.z = 0.75
    pose1.pose.orientation.w = 1

    pose2 = PoseStamped()
    pose2.header.frame_id = 'base_link'
    pose2.pose.position.x = 0.5
    pose2.pose.position.y = 0.3
    pose2.pose.position.z = 0.75
    pose2.pose.orientation.w = 1

    oc = OrientationConstraint()
    oc.header.frame_id = 'base_link'
    oc.link_name = 'wrist_roll_link'
    oc.orientation.w = 1
    oc.absolute_x_axis_tolerance = 0.1
    oc.absolute_y_axis_tolerance = 0.1
    oc.absolute_z_axis_tolerance = 3.14
    oc.weight = 1.0

    gripper = robot_api.Gripper()
    arm = robot_api.Arm()
    def shutdown():
        arm.cancel_all_goals()
    rospy.on_shutdown(shutdown)

    kwargs = {
        'allowed_planning_time': 15,
        'execution_timeout': 10,
        'num_planning_attempts': 5,
        'replan': True,
        'orientation_constraint': oc
    }

    planning_scene.removeAttachedObject('tray')
    gripper.open()

    error = arm.move_to_pose(pose1, **kwargs)
    if error is not None:
        rospy.logerr('Pose 1 failed: {}'.format(error))
    else:
        rospy.loginfo('Pose 1 succeeded')
        rospy.sleep(2)
        frame_attached_to = 'gripper_link'
        frames_okay_to_collide_with = [
            'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link'
        ]
        planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0,
                                frame_attached_to, frames_okay_to_collide_with)
        planning_scene.setColor('tray', 1, 0, 1)
        planning_scene.sendColors()
        gripper.close()

    rospy.sleep(2)

    error = arm.move_to_pose(pose2, **kwargs)
    if error is not None:
        rospy.logerr('Pose 2 failed: {}'.format(error))
    else:
        rospy.loginfo('Pose 2 succeeded')

    gripper.open()
    planning_scene.removeAttachedObject('tray')
    planning_scene.removeCollisionObject('table')
    planning_scene.removeCollisionObject('divider')
    
    return
Ejemplo n.º 14
0
class GraspingClient(object):

    def __init__(self):
        self.scene = PlanningSceneInterface("base_link")
        self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
        self.move_group = MoveGroupInterface("arm", "base_link")

        find_topic = "basic_grasping_perception/find_objects"
        rospy.loginfo("Waiting for %s..." % find_topic)
        self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
        self.find_client.wait_for_server()

    def updateScene(self):
        # find objects
        # here i am declaring goal as an object. https://github.com/mikeferguson/grasping_msgs/blob/master/action/FindGraspableObjects.action
        goal = FindGraspableObjectsGoal()
        goal.plan_grasps = True
        
        # passing the object to find_client
        # now find_client is wer magic happens
        # on demo.launch file i am runnning basic_grasping_perception. (below <!-- Start Perception -->)
        # this keeps running on background and i use actionlib (initalize on init) to get a hook to it.
        # find_client is connected to basic_grasping_perception
        self.find_client.send_goal(goal)
        self.find_client.wait_for_result(rospy.Duration(5.0))
        # here we get all the objects
        find_result = self.find_client.get_result()

        # remove previous objects
        for name in self.scene.getKnownCollisionObjects():
            self.scene.removeCollisionObject(name, False)
        for name in self.scene.getKnownAttachedObjects():
            self.scene.removeAttachedObject(name, False)
        self.scene.waitForSync()

        rospy.loginfo("updating scene")
        idx = 0
        # insert objects to the planning scene
        #TODO so these two for loops yo can hardcode the values. try printing all the params and u will understand
        for obj in find_result.objects:
            rospy.loginfo("object number -> %d" %idx)
            obj.object.name = "object%d"%idx
            self.scene.addSolidPrimitive(obj.object.name,
                                         obj.object.primitives[0],
                                         obj.object.primitive_poses[0],
                                         wait = False)
            
            idx += 1
            # for grp in obj.grasps:
            #     grp.grasp_pose.pose.position.z = 0.37
        # this is just minor adjustments i did mess up with this code. just follwed simple gasp thingy
        for obj in find_result.support_surfaces:
            # extend surface to floor, and make wider since we have narrow field of view
            height = obj.primitive_poses[0].position.z
            rospy.loginfo("height before => %f" % height)
            obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
                                            1.5,  # wider
                                            obj.primitives[0].dimensions[2] + height]
            obj.primitive_poses[0].position.z += -height/2.0

            rospy.loginfo("height after => %f" % obj.primitive_poses[0].position.z)

            # add to scene
            self.scene.addSolidPrimitive(obj.name,
                                         obj.primitives[0],
                                         obj.primitive_poses[0],
                                         wait = False)

        self.scene.waitForSync()

        # store for grasping
        self.objects = find_result.objects
        self.surfaces = find_result.support_surfaces

    def getGraspableCube(self):
        graspable = None
        for obj in self.objects:
            # need grasps
            rospy.loginfo("no. of grasps detected %d dim => %f" % (len(obj.grasps), obj.object.primitives[0].dimensions[0]))
            if len(obj.grasps) < 1:
                continue
            # check size our object is a 0.05^3 cube
            if obj.object.primitives[0].dimensions[0] < 0.04 or \
               obj.object.primitives[0].dimensions[0] > 0.06 or \
               obj.object.primitives[0].dimensions[1] < 0.04 or \
               obj.object.primitives[0].dimensions[1] > 0.06 or \
               obj.object.primitives[0].dimensions[2] < 0.04 or \
               obj.object.primitives[0].dimensions[2] > 0.06:
                continue

            rospy.loginfo("###### size: x =>  %f, y => %f, z => %f" % (obj.object.primitive_poses[0].position.x,obj.object.primitive_poses[0].position.y,obj.object.primitive_poses[0].position.z))
            # has to be on table
            #if obj.object.primitive_poses[0].position.z < 0.5:grasping_client
            #    continue
            return obj.object, obj.grasps
        # nothing detected
        rospy.loginfo("nothing detected")
        return None, None

    def getSupportSurface(self, name):
        for surface in self.support_surfaces:
            if surface.name == name:
                return surface
        return None

    def getPlaceLocation(self):
        pass

    def pick(self, block, grasps):
        success, pick_result = self.pickplace.pick_with_retry(block.name,
                                                              grasps,
                                                              support_name=block.support_surface,
                                                              scene=self.scene)
        self.pick_result = pick_result
        return success

    def place(self, block, pose_stamped):
        places = list()
        l = PlaceLocation()
        l.place_pose.pose = pose_stamped.pose
        l.place_pose.header.frame_id = pose_stamped.header.frame_id

        # copy the posture, approach and retreat from the grasp used
        l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
        l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
        l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
        places.append(copy.deepcopy(l))
        # create another several places, rotate each by 360/m degrees in yaw direction
        m = 16 # number of possible place poses
        pi = 3.141592653589
        for i in range(0, m-1):
            l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m)
            places.append(copy.deepcopy(l))

        success, place_result = self.pickplace.place_with_retry(block.name,
                                                                places,
                                                                scene=self.scene)
        return success

    def tuck(self):
        # so for each joint i will pass a specific value. first arms are move to pose1 then pose2 then pose3 sequentially
        joints = ["shoulder_roll_joint", "shoulder_pitch_joint", "shoulder_yaw_joint", 
                  "elbow_pitch_joint", "elbow_yaw_joint", "wrist_pitch_joint", "wrist_roll_joint"]
        pose1 = [1.57079633, 0, 0, 0, 0, 0, 0.0]
        pose2 = [1.57079633, 0, -1.57079633, 0, -1.57079633, 0, 0.0]
        pose3 = [1.57079633, 1.57079633, -1.57079633, 0, -1.57079633, 1.57079633, 0.0]
        while not rospy.is_shutdown():
            self.move_group.moveToJointPosition(joints, pose1, 0.02)
            self.move_group.moveToJointPosition(joints, pose2, 0.02)
            result = self.move_group.moveToJointPosition(joints, pose3, 0.02)
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                return
Ejemplo n.º 15
0
class World:
    def __init__(self, tower_array, disk_array, config, debug=0):
        print "[INFO] Initialise World"
        self.DEBUG = debug
        # initialise arrays
        self.tower_array = tower_array
        self.disk_array = disk_array
        # configs
        self.max_gripper = config.get_max_gripper()
        self.disk_height = config.disk_height
        self.tower_positions = config.tower_positions

        self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME)
        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene',
                                         PlanningScene,
                                         queue_size=10)

    """Calls a method to display the collision objects.
    """

    def create_planning_scene(self):
        print "[INFO] Create_planning_scene"
        self.display_towers_and_disks()

    """This method collects all needed information and
    publish them to other methods.
    """

    def display_towers_and_disks(self):
        print "[INFO] Display towers and disks"
        for tower in self.tower_array:
            # call method to publish new tower
            self.publish_scene(tower.get_position(), tower)
            # set color by name
            self.planning_scene_interface.setColor(tower.get_name(), 1.0, 1.0,
                                                   1.0)
            # publish color
            self.planning_scene_interface.sendColors()
        for disk in self.disk_array:
            self.publish_scene(disk.get_position(), None, disk)
            self.planning_scene_interface.setColor(disk.get_id(),
                                                   disk.get_color()[0],
                                                   disk.get_color()[1],
                                                   disk.get_color()[2],
                                                   disk.get_color()[3])
            self.planning_scene_interface.sendColors()
        # wait for sync after publishing collision objects
        self.planning_scene_interface.waitForSync(5.0)
        rospy.sleep(5)

    """This method creates a box or a cylinder with methods of
    the planning scene interface.
    :param position: the position of the new collision object
    :param tower: the tower object
    :param disk: the disk object
    """

    def publish_scene(self, position, tower=None, disk=None):
        if tower is not None:
            self.planning_scene_interface.addBox(
                tower.get_name(), self.max_gripper / 100.0,
                self.max_gripper / 100.0,
                (self.tower_positions[tower.get_id() - 1][2] * 2), position[0],
                position[1], position[2])
        else:
            self.planning_scene_interface.addCylinder(disk.get_id(),
                                                      self.disk_height,
                                                      disk.get_diameter() / 2,
                                                      position[0], position[1],
                                                      position[2])

    """This method cleans the planning scene.
    :param close: with this flag a new planning scene objects will be removed
    in sync otherwise the object will be deleted without syncing the scene
    """

    def clean_up(self, close=False):
        if close:
            # get the actual list after game
            self.planning_scene_interface = PlanningSceneInterface(
                REFERENCE_FRAME)
        for name in self.planning_scene_interface.getKnownCollisionObjects():
            if self.DEBUG is 1:
                print("[DEBUG] Removing object %s" % name)
            self.planning_scene_interface.removeCollisionObject(name, False)
        for name in self.planning_scene_interface.getKnownAttachedObjects():
            if self.DEBUG is 1:
                print("[DEBUG] Removing attached object %s" % name)
            self.planning_scene_interface.removeAttachedObject(name, False)
        if close:
            self.planning_scene_interface.waitForSync(5.0)
            pass

    """This method corrects the position of the moved disk.
    :param tower: parent tower of the disk
    """

    def refresh_disk_pose(self, tower):
        print "[INFO] Refresh disk pose"
        disk = tower.get_last()
        if self.DEBUG is 1:
            print "[DEBUG] Refresh", disk.get_id(), "pose:", disk.get_position(), "tower size", tower.get_size(),\
                "tower pose", tower.get_position()
        # remove the disk from planning scene
        self.planning_scene_interface.removeCollisionObject(
            disk.get_id(), False)
        # publish collision object and set old color
        self.publish_scene(disk.get_position(), None, disk)
        self.planning_scene_interface.setColor(disk.get_id(),
                                               disk.get_color()[0],
                                               disk.get_color()[1],
                                               disk.get_color()[2],
                                               disk.get_color()[3])
        self.planning_scene_interface.sendColors()
Ejemplo n.º 16
0
    def __init__(self):

        self.moving_mode = False
        self.waldo_mode = False
        self.prev_joy_buttons = 0

        # See http://docs.fetchrobotics.com/robot_hardware.html#naming-conventions
        self.joint_names = [
            #'torso_lift_joint',
            #'bellows_joint',
            #'head_pan_joint',
            #'head_tilt_joint',
            'shoulder_pan_joint',
            'shoulder_lift_joint',
            'upperarm_roll_joint',
            'elbow_flex_joint',
            'forearm_roll_joint',
            'wrist_flex_joint',
            'wrist_roll_joint',
            'l_gripper_finger_joint',
            #'r_gripper_finger_joint',
            ]
        self.joint_name_map = dict([(name, index) for index, name in enumerate(self.joint_names)])
        self.cur_joint_pos = np.zeros([len(self.joint_names)])
        self.cur_joint_vel = np.zeros([len(self.joint_names)])
        self.cur_joint_effort = np.zeros([len(self.joint_names)])

        self.cur_base_scan = np.zeros([662], dtype=np.float32)
        self.cur_head_camera_depth_image = np.zeros([480,640], dtype=np.float32)
        self.cur_head_camera_rgb_image = np.zeros([480,640,3], dtype=np.uint8)

        self.timeseq = None
        self.timeseq_mutex = threading.Lock()
        self.image_queue = Queue.Queue()
        self.image_compress_thread = threading.Thread(target=self.image_compress_main)
        self.image_compress_thread.daemon = True
        self.image_compress_thread.start()

        logger.info('Subscribing...')
        self.subs = []
        if 1: self.subs.append(rospy.Subscriber('/joint_states', JointState, self.joint_states_cb))
        if 0: self.subs.append(rospy.Subscriber('/base_scan', LaserScan, self.base_scan_cb))
        if 1: self.subs.append(rospy.Subscriber('/head_camera/depth/image', numpy_msg(Image), self.head_camera_depth_image_cb))
        if 1: self.subs.append(rospy.Subscriber('/head_camera/rgb/image_raw', numpy_msg(Image), self.head_camera_rgb_image_raw_cb))
        if 1: self.subs.append(rospy.Subscriber('/spacenav/joy', Joy, self.spacenav_joy_cb))
        logger.info('Subscribed')

        self.arm_effort_pub = rospy.Publisher('/arm_controller/weightless_torque/command', JointTrajectory, queue_size=2)
        #self.head_goal_pub = rospy.Publisher('/head_controller/point_head/goal', PointHeadActionGoal, queue_size=2)
        self.gripper_client = actionlib.SimpleActionClient('gripper_controller/gripper_action', GripperCommandAction)

        self.arm_cartesian_twist_pub = rospy.Publisher('/arm_controller/cartesian_twist/command', Twist, queue_size=2)


        self.head_point_client = actionlib.SimpleActionClient('head_controller/point_head', PointHeadAction)

        self.arm_move_group = MoveGroupInterface("arm", "base_link", plan_only = True)
        self.arm_trajectory_client = actionlib.SimpleActionClient("arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
        self.arm_trajectory_client.wait_for_server()


        if 0:
            logger.info('Creating MoveGroupInterface...')
            self.move_group = MoveGroupInterface('arm_with_torso', 'base_link', plan_only = True)
            logger.info('Created MoveGroupInterface')
            if 0:
                logger.info('Creating PlanningSceneInterface...')
                self.planning_scene = PlanningSceneInterface('base_link')
                self.planning_scene.removeCollisionObject('my_front_ground')
                self.planning_scene.removeCollisionObject('my_back_ground')
                self.planning_scene.removeCollisionObject('my_right_ground')
                self.planning_scene.removeCollisionObject('my_left_ground')
                self.planning_scene.addCube('my_front_ground', 2, 1.1, 0.0, -1.0)
                self.planning_scene.addCube('my_back_ground', 2, -1.2, 0.0, -1.0)
                self.planning_scene.addCube('my_left_ground', 2, 0.0, 1.2, -1.0)
                self.planning_scene.addCube('my_right_ground', 2, 0.0, -1.2, -1.0)


        logger.warn('FetchRobotGymEnv ROS node running')

        self.head_point_client.wait_for_server()
        logger.warn('FetchRobotGymEnv ROS node connected')
class GripperInterfaceClient:
    def __init__(self):
        self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"]

        self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction)
        rospy.loginfo("Waiting for gripper_controller...")
        self.client.wait_for_server()
        rospy.loginfo("...connected.")

        self.scene = PlanningSceneInterface("base_link")
        self.attached_object_publisher = rospy.Publisher(
            "attached_collision_object", AttachedCollisionObject, queue_size=10
        )
        self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True)

        find_topic = "basic_grasping_perception/find_objects"
        rospy.loginfo("Waiting for %s..." % find_topic)
        self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
        self.find_client.wait_for_server()
        self.debug_index = -1

        self.graspables = []

    def execute_trajectory(self, trajectory):
        goal_position = trajectory.points[-1].positions
        print "==========", goal_position
        command_goal = GripperCommandGoal()
        command_goal.command.position = goal_position[0] + goal_position[1]
        command_goal.command.max_effort = 50.0  # Placeholder. TODO Figure out a better way to compute this
        self.client.send_goal(command_goal)
        self.client.wait_for_result()

    def move_to(self, goal_position, max_effort):
        command_goal = GripperCommandGoal()
        command_goal.command.position = goal_position
        command_goal.command.max_effort = max_effort
        self.client.send_goal(command_goal)
        self.client.wait_for_result()

    def updateScene(self):
        # find objects
        goal = FindGraspableObjectsGoal()
        goal.plan_grasps = True
        self.find_client.send_goal(goal)
        self.find_client.wait_for_result(rospy.Duration(5.0))
        find_result = self.find_client.get_result()

        # remove previous objects
        for name in self.scene.getKnownCollisionObjects():
            self.scene.removeCollisionObject(name, wait=False)

        for name in self.scene.getKnownAttachedObjects():
            self.scene.removeAttachedObject(name, wait=False)

        rospy.sleep(0.5)  # Gets rid of annoying error messages stemming from race condition.

        self.scene.waitForSync()

        # insert objects to scene
        idx = -1
        for obj in find_result.objects:
            idx += 1
            obj.object.name = "object%d" % idx
            self.scene.addSolidPrimitive(
                obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait=False
            )

        for obj in find_result.support_surfaces:
            # extend surface to floor, and make wider since we have narrow field of view
            height = obj.primitive_poses[0].position.z
            obj.primitives[0].dimensions = [
                obj.primitives[0].dimensions[0] + 0.1,
                1.5,  # wider
                obj.primitives[0].dimensions[2] + height,
            ]
            obj.primitive_poses[0].position.z += -height / 2.0

            # add to scene
            self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait=False)

        self.scene.waitForSync()
        # store for grasping
        self.objects = find_result.objects
        self.surfaces = find_result.support_surfaces

    def getGraspableCube(self):
        graspable = None
        for obj in self.objects:
            # need grasps
            if len(obj.grasps) < 1:
                continue
            # check size
            # if obj.object.primitives[0].dimensions[0] < 0.05 or \
            #   obj.object.primitives[0].dimensions[0] > 0.07 or \
            #   obj.object.primitives[0].dimensions[0] < 0.05 or \
            #   obj.object.primitives[0].dimensions[0] > 0.07 or \
            #   obj.object.primitives[0].dimensions[0] < 0.05 or \
            #   obj.object.primitives[0].dimensions[0] > 0.07:
            #    continue
            # has to be on table
            # if obj.object.primitive_poses[0].position.z < 0.5:
            #    continue
            return obj.object, obj.grasps
        # nothing detected
        return None, None

    def set_target_object(self, primitive_type, target_pose, dimensions):
        primitive_types = {"BOX": 1, "SPHERE": 2, "CYLINDER": 3, "CONE": 4}
        self.sought_object = Object()
        primitive = SolidPrimitive()
        primitive.type = primitive_types[primitive_type.upper()]
        primitive.dimensions = dimensions
        self.sought_object.primitives.append(primitive)

        p = Pose()
        p.position.x = target_pose[0]
        p.position.y = target_pose[1]
        p.position.z = target_pose[2]

        quaternion = quaternion_from_euler(target_pose[3], target_pose[4], target_pose[5])
        p.orientation.x = quaternion[0]
        p.orientation.y = quaternion[1]
        p.orientation.z = quaternion[2]
        p.orientation.w = quaternion[3]

        self.sought_object.primitive_poses.append(p)

    def find_graspable_object(self, tolerance=0.01):
        self.updateScene()
        self.current_grasping_target = None
        for obj in self.objects:
            # Must have grasps
            if len(obj.grasps) < 1:
                continue
            # Compare to sought object
            detected_object_dimensions = np.array(obj.object.primitives[0].dimensions)
            sought_object_dimensions = np.array(self.sought_object.primitives[0].dimensions)
            try:
                difference = detected_object_dimensions - sought_object_dimensions
                print "===DEBUG: Difference: ", difference
                mag_diff = np.linalg.norm(difference)
                print "===DEBUG: mag_diff: ", mag_diff
                if mag_diff <= tolerance:
                    self.current_grasping_target = obj
                    tolerance = mag_diff
                    print "===DEBUG: Tolerance: ", tolerance
                else:
                    rospy.loginfo("Object dimensions do not match. Trying another object...")
            except:
                rospy.loginfo("Object geometries do not match. Trying another object...")
        if self.current_grasping_target is None:
            tolerance += 0.01
            self.find_graspable_object(tolerance=tolerance)
        # Nothing detected

    def computeGraspToPickMatrix(self):
        pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation
        grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation
        pick_translation_distance = self.pick_result.grasp.pre_grasp_approach.desired_distance
        pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w]
        grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w]
        pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion)
        rotation_matrix = quaternion_matrix(pick_to_grasp_quaternion)

        translation = [pick_translation_distance, 0, 0]
        rotation_matrix[[0, 1, 2], 3] = translation
        pick_to_grasp_matrix = np.mat(rotation_matrix)
        grasp_to_pick_matrix = pick_to_grasp_matrix.getI()
        return grasp_to_pick_matrix

    def computePlaceToBaseMatrix(self, place):
        place_quaternion = place[3:]
        rotation_matrix = quaternion_matrix(place_quaternion)
        translation = place[0:3]
        rotation_matrix[[0, 1, 2], 3] = translation
        place_to_base_matrix = rotation_matrix
        return place_to_base_matrix

    def broadcastCurrentGraspingTargetTransform(self):
        pose = self.current_grasping_target.object.primitive_poses[0]
        br = tf2_ros.TransformBroadcaster()

        t = geometry_msgs.msg.TransformStamped()
        t.header.stamp = rospy.Time.now()
        t.header.frame_id = "base_link"
        t.child_frame_id = "current_grasping_target"

        t.transform.translation.x = pose.position.x
        t.transform.translation.y = pose.position.y
        t.transform.translation.z = pose.position.z
        t.transform.rotation.x = pose.orientation.x
        t.transform.rotation.y = pose.orientation.y
        t.transform.rotation.z = pose.orientation.z
        t.transform.rotation.w = pose.orientation.w

        br.sendTransform(t)

    def plan_pick(self):
        success, pick_result = self.pickplace.pick_with_retry(
            self.current_grasping_target.object.name,
            self.current_grasping_target.grasps,
            support_name=self.current_grasping_target.object.support_surface,
            scene=self.scene,
            allow_gripper_support_collision=False,
            planner_id="PRMkConfigDefault",
        )
        self.pick_result = pick_result
        print "DEBUG: plan_pick(): ", self.scene.getKnownAttachedObjects()
        if success:
            rospy.loginfo("Planning succeeded.")
            trajectory_approved = get_user_approval()
            if trajectory_approved:
                return pick_result.trajectory_stages
            else:
                rospy.loginfo("Plan rejected. Getting new grasps for replan...")
        else:
            rospy.logwarn("Planning failed. Getting new grasps for replan...")
        self.find_graspable_object()
        return self.plan_pick()

    def plan_place(self, desired_pose):
        places = list()
        ps = PoseStamped()
        # ps.pose = self.current_grasping_target.object.primitive_poses[0]
        ps.header.frame_id = self.current_grasping_target.object.header.frame_id

        grasp_to_pick_matrix = self.computeGraspToPickMatrix()
        place_to_base_matrix = self.computePlaceToBaseMatrix(desired_pose)
        grasp2_to_place_matrix = place_to_base_matrix * grasp_to_pick_matrix
        position_vector = grasp2_to_place_matrix[0:-1, 3]
        quaternion = quaternion_from_matrix(grasp2_to_place_matrix)
        position_array = position_vector.getA1()

        l = PlaceLocation()
        direction_components = self.pick_result.grasp.pre_grasp_approach.direction.vector
        l.place_pose.header.frame_id = ps.header.frame_id
        l.place_pose.pose.position.x = position_array[0]
        l.place_pose.pose.position.y = position_array[1]
        l.place_pose.pose.position.z = position_array[2]
        l.place_pose.pose.orientation.x = quaternion[0]
        l.place_pose.pose.orientation.y = quaternion[1]
        l.place_pose.pose.orientation.z = quaternion[2]
        l.place_pose.pose.orientation.w = quaternion[3]

        # copy the posture, approach and retreat from the grasp used
        approach = copy.deepcopy(self.pick_result.grasp.pre_grasp_approach)
        approach.desired_distance /= 2.0

        l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
        l.pre_place_approach = approach
        l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
        places.append(copy.deepcopy(l))
        # create another several places, rotate each by 90 degrees in roll direction
        l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
        places.append(copy.deepcopy(l))
        l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
        places.append(copy.deepcopy(l))
        l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
        places.append(copy.deepcopy(l))
        print "DEBUG: Places: ", places[0]
        success, place_result = self.pickplace.place_with_retry(
            self.current_grasping_target.object.name,
            places,
            scene=self.scene,
            allow_gripper_support_collision=False,
            planner_id="PRMkConfigDefault",
        )
        if success:
            rospy.loginfo("Planning succeeded.")
            trajectory_approved = get_user_approval()
            if trajectory_approved:
                return place_result.trajectory_stages
            else:
                rospy.loginfo("Plan rejected. Replanning...")
        else:
            rospy.logwarn("Planning failed. Replanning...")
        desired_pose[2] += 0.05
        return self.plan_place(desired_pose)

    def recognizeAttachedObject(self):
        print "========= Recognizing attached object"
        name = self.current_grasping_target.object.name
        size_x = self.current_grasping_target.object.primitives[0].dimensions[0]
        size_y = self.current_grasping_target.object.primitives[0].dimensions[1]
        size_z = self.current_grasping_target.object.primitives[0].dimensions[2]
        (x, y, z) = (0.03, 0.0, 0.0)
        link_name = "gripper_link"
        touch_links = ["l_gripper_finger_link", "r_gripper_finger_link", "gripper_link"]
        detach_posture = None
        weight = 0.0
        wait = True
        """ 
        object_x = self.current_grasping_target.object.primitive_poses[0].position.x
        object_y = self.current_grasping_target.object.primitive_poses[0].position.y
        object_z = self.current_grasping_target.object.primitive_poses[0].position.z


        pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation
        grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation 
        grasp_position = self.pick_result.grasp.grasp_pose.pose.position
        pick_translation_distance = self.pick_result.grasp.post_grasp_retreat.desired_distance
        pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w]
        grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w]
        pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion)

        grasp_to_base_matrix_array = quaternion_matrix(grasp_quaternion)
        grasp_to_base_matrix = np.matrix(grasp_to_base_matrix_array)
        displacement = grasp_to_base_matrix.dot(np.matrix([[pick_translation_distance-.03], [0], [0], [0]]))
        print displacement

        attached_location = list()
        attached_location.append(object_x - displacement[0,0])
        attached_location.append(object_y - displacement[1,0])
        attached_location.append(object_z - displacement[2,0])       
        print attached_location
        

        quaternion = Quaternion()
        quaternion.x = pick_to_grasp_quaternion[0]
        quaternion.y = pick_to_grasp_quaternion[1]
        quaternion.z = pick_to_grasp_quaternion[2]
        quaternion.w = pick_to_grasp_quaternion[3]

        pose = Pose()
        pose.position.x = attached_location[0]
        pose.position.y = attached_location[1]
        pose.position.z = attached_location[2]

        pose.orientation = self.current_grasping_target.object.primitive_poses[0].orientation


        collision_object = self.scene.makeSolidPrimitive(self.current_grasping_target.object.name,
                                                     self.current_grasping_target.object.primitives[0],
                                                     pose)
                                                         
                                                          
        attached_collision_object = self.scene.makeAttached("gripper_link",
                                                       collision_object,
                                                       touch_links, detach_posture, 0.0)
        

        self.attached_object_publisher.publish(attached_collision_object)
        """
        self.scene.attachBox(
            name,
            size_x,
            size_y,
            size_z,
            x,
            y,
            z,
            link_name,
            touch_links=touch_links,
            detach_posture=detach_posture,
            weight=weight,
            wait=wait,
        )

    def removeCollisionObjects(self):
        collision_object_names = self.scene.getKnownCollisionObjects()
        print self.current_grasping_target.object.name
        print collision_object_names
        raw_input("press ENTER")
        x = self.scene.getKnownAttachedObjects()
        print x
        raw_input("press Enter")
        for name in collision_object_names:
            if name != self.current_grasping_target.object.name:
                self.scene.removeCollisionObject(name, wait=False)
Ejemplo n.º 18
0
class FetchRobotApi:
    def __init__(self):

        self.moving_mode = False
        self.waldo_mode = False
        self.prev_joy_buttons = 0

        # See http://docs.fetchrobotics.com/robot_hardware.html#naming-conventions
        self.joint_names = [
            #'torso_lift_joint',
            #'bellows_joint',
            #'head_pan_joint',
            #'head_tilt_joint',
            'shoulder_pan_joint',
            'shoulder_lift_joint',
            'upperarm_roll_joint',
            'elbow_flex_joint',
            'forearm_roll_joint',
            'wrist_flex_joint',
            'wrist_roll_joint',
            'l_gripper_finger_joint',
            #'r_gripper_finger_joint',
            ]
        self.joint_name_map = dict([(name, index) for index, name in enumerate(self.joint_names)])
        self.cur_joint_pos = np.zeros([len(self.joint_names)])
        self.cur_joint_vel = np.zeros([len(self.joint_names)])
        self.cur_joint_effort = np.zeros([len(self.joint_names)])

        self.cur_base_scan = np.zeros([662], dtype=np.float32)
        self.cur_head_camera_depth_image = np.zeros([480,640], dtype=np.float32)
        self.cur_head_camera_rgb_image = np.zeros([480,640,3], dtype=np.uint8)

        self.timeseq = None
        self.timeseq_mutex = threading.Lock()
        self.image_queue = Queue.Queue()
        self.image_compress_thread = threading.Thread(target=self.image_compress_main)
        self.image_compress_thread.daemon = True
        self.image_compress_thread.start()

        logger.info('Subscribing...')
        self.subs = []
        if 1: self.subs.append(rospy.Subscriber('/joint_states', JointState, self.joint_states_cb))
        if 0: self.subs.append(rospy.Subscriber('/base_scan', LaserScan, self.base_scan_cb))
        if 1: self.subs.append(rospy.Subscriber('/head_camera/depth/image', numpy_msg(Image), self.head_camera_depth_image_cb))
        if 1: self.subs.append(rospy.Subscriber('/head_camera/rgb/image_raw', numpy_msg(Image), self.head_camera_rgb_image_raw_cb))
        if 1: self.subs.append(rospy.Subscriber('/spacenav/joy', Joy, self.spacenav_joy_cb))
        logger.info('Subscribed')

        self.arm_effort_pub = rospy.Publisher('/arm_controller/weightless_torque/command', JointTrajectory, queue_size=2)
        #self.head_goal_pub = rospy.Publisher('/head_controller/point_head/goal', PointHeadActionGoal, queue_size=2)
        self.gripper_client = actionlib.SimpleActionClient('gripper_controller/gripper_action', GripperCommandAction)

        self.arm_cartesian_twist_pub = rospy.Publisher('/arm_controller/cartesian_twist/command', Twist, queue_size=2)


        self.head_point_client = actionlib.SimpleActionClient('head_controller/point_head', PointHeadAction)

        self.arm_move_group = MoveGroupInterface("arm", "base_link", plan_only = True)
        self.arm_trajectory_client = actionlib.SimpleActionClient("arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
        self.arm_trajectory_client.wait_for_server()


        if 0:
            logger.info('Creating MoveGroupInterface...')
            self.move_group = MoveGroupInterface('arm_with_torso', 'base_link', plan_only = True)
            logger.info('Created MoveGroupInterface')
            if 0:
                logger.info('Creating PlanningSceneInterface...')
                self.planning_scene = PlanningSceneInterface('base_link')
                self.planning_scene.removeCollisionObject('my_front_ground')
                self.planning_scene.removeCollisionObject('my_back_ground')
                self.planning_scene.removeCollisionObject('my_right_ground')
                self.planning_scene.removeCollisionObject('my_left_ground')
                self.planning_scene.addCube('my_front_ground', 2, 1.1, 0.0, -1.0)
                self.planning_scene.addCube('my_back_ground', 2, -1.2, 0.0, -1.0)
                self.planning_scene.addCube('my_left_ground', 2, 0.0, 1.2, -1.0)
                self.planning_scene.addCube('my_right_ground', 2, 0.0, -1.2, -1.0)


        logger.warn('FetchRobotGymEnv ROS node running')

        self.head_point_client.wait_for_server()
        logger.warn('FetchRobotGymEnv ROS node connected')

    def start_timeseq(self, script):
        timeseq = TimeseqWriter.create(script)
        timeseq.add_channel('robot_joints', 'FetchRobotJoints')
        timeseq.add_channel('gripper_joints', 'FetchGripperJoints')

        timeseq.add_schema('FetchRobotJoints',
            ('torso_lift_joint', 'JointState'),
            ('head_pan_joint', 'JointState'),
            ('head_tilt_joint', 'JointState'),
            ('shoulder_pan_joint', 'JointState'),
            ('shoulder_lift_joint', 'JointState'),
            ('upperarm_roll_joint', 'JointState'),
            ('elbow_flex_joint', 'JointState'),
            ('forearm_roll_joint', 'JointState'),
            ('wrist_flex_joint', 'JointState'),
            ('wrist_roll_joint', 'JointState'),
        )
        timeseq.add_schema('FetchGripperJoints',
            ('l_gripper_finger_joint', 'JointState'),
        )
        timeseq.add_schema('JointState',
            ('pos', 'double'),
            ('vel', 'double'),
            ('effort', 'double'),
        )
        timeseq.add_channel('arm_action', 'FetchArmWeightlessTorqueAction')
        timeseq.add_schema('FetchArmWeightlessTorqueAction',
            ('shoulder_pan_joint', 'WeightlessTorqueAction'),
            ('shoulder_lift_joint', 'WeightlessTorqueAction'),
            ('upperarm_roll_joint', 'WeightlessTorqueAction'),
            ('elbow_flex_joint', 'WeightlessTorqueAction'),
            ('forearm_roll_joint', 'WeightlessTorqueAction'),
            ('wrist_flex_joint', 'WeightlessTorqueAction'),
            ('wrist_roll_joint', 'WeightlessTorqueAction'),
        )
        timeseq.add_schema('WeightlessTorqueAction',
            ('action', 'double'),
            ('effort', 'double'),
        )

        timeseq.add_channel('gripper_action', 'FetchGripperAction')
        timeseq.add_schema('FetchGripperAction',
            ('action', 'double'),
            ('effort', 'double'),
            ('pos', 'double'),
        )

        timeseq.add_channel('head_camera_rgb', 'VideoFrame')
        timeseq.add_schema('VideoFrame',
            ('url', 'string'),
        )
        with self.timeseq_mutex:
            if self.timeseq:
                self.timeseq.close()
            self.timeseq = timeseq

    def close_timeseq(self):
        with self.timeseq_mutex:
            if self.timeseq is not None:
                self.timeseq.close()
                threading.Thread(target=self.timeseq.upload_s3).start()
                self.timeseq = None

    def start_axis_video(self):
        cameras = rospy.get_param('/axis_cameras')
        if cameras and len(cameras):
            with self.timeseq_mutex:
                if self.timeseq:
                    for name, info in cameras.items():
                        logging.info('Camera %s: %s', name, repr(info))
                        self.timeseq.start_axis_video(timeseq_name = name,
                            auth_header=info.get('auth_header'),
                            daemon_endpoint=info.get('daemon_endpoint'),
                            ipaddr=info.get('ipaddr'),
                            local_link_prefix=info.get('local_link_prefix'),
                            remote_traces_dir=info.get('remote_traces_dir'),
                            resolution=info.get('resolution'))

    def stop_axis_video(self):
        with self.timeseq_mutex:
            if self.timeseq:
                self.timeseq.stop_axis_video()


    def base_scan_cb(self, data):
        # fmin replaces nans with 15
        self.cur_base_scan = np.fmin(np.array(data.ranges), 15.0)


    def head_camera_depth_image_cb(self, data):
        shape = [data.height, data.width]
        dtype = np.dtype(np.float32)
        npdata = np.fromstring(data.data, dtype=dtype).reshape(shape)
        npdata.strides = (data.step, dtype.itemsize)
        self.cur_head_camera_depth_image = np.fmin(npdata, 5.0)

    def head_camera_rgb_image_raw_cb(self, data):
        shape = [data.height, data.width, 3]
        dtype = np.dtype(np.uint8)
        npdata = np.fromstring(data.data, dtype=dtype).reshape(shape)
        npdata.strides = (data.step, dtype.itemsize*3, 1)
        self.cur_head_camera_rgb_image = npdata

        if self.timeseq:
            self.image_queue.put(('head_camera_rgb', data))


    def joint_states_cb(self, data):
        """
        Handle a joint_states message. Messages can have subsets of the joints, hopefully non-overlapping.
        """
        t0 = time.time()
        for i in range(len(data.name)):
            name = data.name[i]
            jni = self.joint_name_map.get(name, -1)
            if jni >= 0:
                self.cur_joint_pos[jni] = data.position[i]
                self.cur_joint_vel[jni] = data.velocity[i]
                self.cur_joint_effort[jni] = data.effort[i]

        with self.timeseq_mutex:
            if self.timeseq:
                channel, channel_type = ((data.name[0] == 'l_wheel_joint' and ('robot_joints', 'FetchRobotJoints')) or
                                         (data.name[0] == 'l_gripper_finger_joint' and ('gripper_joints', 'FetchGripperJoints')) or
                                         (None, None))
                if channel:
                    state = dict([(jn, {
                        '__type': 'JointState',
                        'pos': data.position[i],
                        'vel': data.velocity[i],
                        'effort': data.effort[i],
                        }) for i, jn in enumerate(data.name)])
                    state['__type'] = channel_type
                    state['rxtime'] = t0
                    self.timeseq.add(data.header.stamp.to_sec(), channel, state)
                if 0:
                    t1 = time.time()
                    print '%0.6f+%0.6f stamp=%0.6f (age %0.6f) from %s' % (t0, t1-t0, data.header.stamp.to_sec(), data.header.stamp.to_sec()-t0, channel)


    def image_compress_main(self):
        while True:
            channel, data = self.image_queue.get()
            if 0: print 'len(data.data)=%d data.width=%d data.height=%d' % (len(data.data), data.width, data.height)
            im = PIL.Image.frombytes('RGB', (data.width, data.height), data.data, 'raw')
            jpeg_writer = StringIO()
            im.save(jpeg_writer, 'jpeg')
            im_url = 'data:image/jpeg;base64,' + base64.b64encode(jpeg_writer.getvalue())
            with self.timeseq_mutex:
                if self.timeseq:
                    self.timeseq.add(data.header.stamp.to_sec(), channel, {
                        '__type': 'VideoFrame',
                        'url': im_url,
                    })

    def move_to_start(self):

        self.moving_mode = True
        # Look down
        goal = PointHeadGoal()
        goal.target.header.stamp = rospy.Time.now()
        goal.target.header.frame_id = '/base_link'
        goal.target.point.x = 1.5
        goal.target.point.y = 0.0
        goal.target.point.z = -0.2
        goal.min_duration = rospy.Duration(0.5)
        if 0: logger.info('Point head to %s...', goal);
        self.head_point_client.send_goal(goal)
        if 0: logger.info('Point head sent')

        goal = GripperCommandGoal()
        goal.command.max_effort = 60
        goal.command.position = 0.1
        self.gripper_client.send_goal(goal)

        joints = [
            'shoulder_pan_joint',
            'shoulder_lift_joint',
            'upperarm_roll_joint',
            'elbow_flex_joint',
            'forearm_roll_joint',
            'wrist_flex_joint',
            'wrist_roll_joint',
        ]
        pose = [0.0, +0.8, 0.0, -0.8, 0.0, 0.0, 0.0]
        result = self.arm_move_group.moveToJointPosition(joints, pose, plan_only=True)
        if result.error_code.val == MoveItErrorCodes.SUCCESS:
            if 0: logger.info('Got trajectory %s', result.planned_trajectory)
            follow_goal = FollowJointTrajectoryGoal()
            follow_goal.trajectory = result.planned_trajectory.joint_trajectory
            logger.info('sending trajectory to arm...')
            self.arm_trajectory_client.send_goal(follow_goal)
            result = self.arm_trajectory_client.wait_for_result()
            logger.info('arm followed trajectory %s', result)
        else:
            logger.warn('moveToJointPosition returned %s', result)
            return

        result = self.head_point_client.wait_for_result()
        logger.info('head followed trajectory %s', result)

        logger.info('sending empty arm goal')
        empty_goal = FollowJointTrajectoryGoal()
        self.arm_trajectory_client.send_goal(empty_goal)

        logger.info('Point head done')
        self.moving_mode = False


    def set_arm_action(self, action):
        arm_joints = [
            ('shoulder_pan_joint', 1.57, 33.82),
            ('shoulder_lift_joint', 1.57, 131.76),
            ('upperarm_roll_joint', 1.57, 76.94),
            ('elbow_flex_joint', 1.57, 66.18),
            ('forearm_roll_joint', 1.57, 29.35),
            ('wrist_flex_joint', 2.26, 25.70),
            ('wrist_roll_joint', 2.26, 7.36),
        ]
        arm_efforts = [min(1.0, max(-1.0, action[self.joint_name_map.get(name)])) * torque_scale * 0.75 for name, vel_scale, torque_scale in arm_joints]
        arm_joint_names = [name for name, vel_scale, torque_scale in arm_joints]
        if 1:
            arm_joint_names.append('gravity_compensation')
            arm_efforts.append(1.0)
        arm_msg = JointTrajectory(joint_names=arm_joint_names, points=[JointTrajectoryPoint(effort = arm_efforts)])
        self.arm_effort_pub.publish(arm_msg)

        with self.timeseq_mutex:
            if self.timeseq:
                state = dict([(jn, {
                    '__type': 'WeightlessTorqueAction',
                    'action': action[self.joint_name_map.get(jn)],
                    'effort': arm_efforts[i],
                    }) for i, jn in enumerate(arm_joint_names)])
                state['__type'] = 'FetchArmWeightlessTorqueAction'
                self.timeseq.add(time.time(), 'arm_action', state)

    def set_gripper_action(self, action):

        grip = action[self.joint_name_map.get('l_gripper_finger_joint')]

        goal = GripperCommandGoal()
        if grip > 0:
            goal.command.max_effort = 60*min(1.0, grip)
            goal.command.position = 0.0
        else:
            goal.command.max_effort = 60
            goal.command.position = 0.1

        self.gripper_client.send_goal(goal)

        with self.timeseq_mutex:
            if self.timeseq:
                state = {
                    '__type': 'FetchGripperAction',
                    'action': grip,
                    'effort': goal.command.max_effort,
                    'pos': goal.command.position,
                }
                self.timeseq.add(time.time(), 'gripper_action', state)

    def set_waldo_action(self, joy):
        twist = Twist()
        twist.linear.x = joy.axes[0]
        twist.linear.y = joy.axes[1]
        twist.linear.z = joy.axes[2]
        twist.angular.x = +3.0*joy.axes[3]
        twist.angular.y = +3.0*joy.axes[4]
        twist.angular.z = +2.0*joy.axes[5]
        self.arm_cartesian_twist_pub.publish(twist)

        if joy.buttons[1] and not self.prev_joy_buttons[1]:
            goal = GripperCommandGoal()
            goal.command.max_effort = 60
            goal.command.position = 0.0
            self.gripper_client.send_goal(goal)
        elif not joy.buttons[1] and self.prev_joy_buttons[1]:
            goal = GripperCommandGoal()
            goal.command.max_effort = 60
            goal.command.position = 0.1
            self.gripper_client.send_goal(goal)

    def spacenav_joy_cb(self, joy):
        if 0: logger.info('joy %s', str(joy))
        if joy.buttons[0] and not self.prev_joy_buttons[0]:
            if self.waldo_mode:
                self.stop_waldo_mode()
            elif not self.moving_mode:
                self.start_waldo_mode()

        if self.waldo_mode and not self.moving_mode:
            self.set_waldo_action(joy)
        self.prev_joy_buttons = joy.buttons

    def start_waldo_mode(self):
        logger.info('Start waldo mode');
        self.waldo_mode = True
        self.start_timeseq('fetchwaldo')
        self.start_axis_video()

    def stop_waldo_mode(self):
        logger.info('Stop waldo mode');
        self.waldo_mode = False
        timeseq_url = None
        if self.timeseq:
            timeseq_url = self.timeseq.get_url()
        logger.info('save_logs url=%s', timeseq_url)
        self.stop_axis_video()
        self.close_timeseq()

        mts_thread = threading.Thread(target=self.move_to_start)
        mts_thread.daemon = True
        mts_thread.start()
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)
        
        rospy.init_node('moveit_demo')
                
        # Use the planning scene object to add or remove objects
        scene = PlanningSceneInterface("base_link")
        
        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
        
        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
        
        # Create a dictionary to hold object colors
        self.colors = dict()
                        
        # Initialize the move group for the right arm
        right_arm = MoveGroupCommander(GROUP_NAME_ARM)
        
        # Initialize the move group for the right gripper
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
 
        # Allow some leeway in position (meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.05)
        right_arm.set_goal_orientation_tolerance(0.1)

        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)
        
        # Set the right arm reference frame
        right_arm.set_pose_reference_frame(REFERENCE_FRAME)
        
        # Allow 5 seconds per planning attempt
        right_arm.set_planning_time(15)
        
        # Set a limit on the number of pick attempts before bailing
        max_pick_attempts = 5
        
        # Set a limit on the number of place attempts
        max_place_attempts = 3
                
        # Give the scene a chance to catch up
        rospy.sleep(2)
        
        # Connect to the UBR-1 find_objects action server
        rospy.loginfo("Connecting to basic_grasping_perception/find_objects...")
        find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction)
        find_objects.wait_for_server()
        rospy.loginfo("...connected")
        
        # Give the scene a chance to catch up    
        rospy.sleep(1)
        
        # Start the arm in the "resting" pose stored in the SRDF file
        right_arm.set_named_target('resting')
        right_arm.go()
        
        # Open the gripper to the neutral position
        right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        right_gripper.go()
       
        rospy.sleep(1)
        
        # Begin the main perception and pick-and-place loop
        while not rospy.is_shutdown():
            # Initialize the grasping goal
            goal = FindGraspableObjectsGoal()
            
            # We don't use the UBR-1 grasp planner as it does not work with our gripper
            goal.plan_grasps = False
            
            # Send the goal request to the find_objects action server which will trigger
            # the perception pipeline
            find_objects.send_goal(goal)
            
            # Wait for a result
            find_objects.wait_for_result(rospy.Duration(5.0))
            
            # The result will contain support surface(s) and objects(s) if any are detected
            find_result = find_objects.get_result()
    
            # Display the number of objects found
            rospy.loginfo("Found %d objects" % len(find_result.objects))
    
            # Remove all previous objects from the planning scene
            for name in scene.getKnownCollisionObjects():
                scene.removeCollisionObject(name, False)
            for name in scene.getKnownAttachedObjects():
                scene.removeAttachedObject(name, False)
            scene.waitForSync()
            
            # Clear the virtual object colors
            scene._colors = dict()
    
            # Use the nearest object on the table as the target
            target_pose = PoseStamped()
            target_pose.header.frame_id = REFERENCE_FRAME
            target_size = None
            the_object = None
            the_object_dist = 1.0
            count = -1
            
            # Cycle through all detected objects and keep the nearest one
            for obj in find_result.objects:
                count += 1
                scene.addSolidPrimitive("object%d"%count,
                                        obj.object.primitives[0],
                                        obj.object.primitive_poses[0],
                                        wait = False)

                # Choose the object nearest to the robot
                dx = obj.object.primitive_poses[0].position.x - args.x
                dy = obj.object.primitive_poses[0].position.y
                d = math.sqrt((dx * dx) + (dy * dy))
                if d < the_object_dist:
                    the_object_dist = d
                    the_object = count
                    
                    # Get the size of the target
                    target_size = obj.object.primitives[0].dimensions
                    
                    # Set the target pose
                    target_pose.pose = obj.object.primitive_poses[0]
                    
                    # We want the gripper to be horizontal
                    target_pose.pose.orientation.x = 0.0
                    target_pose.pose.orientation.y = 0.0
                    target_pose.pose.orientation.z = 0.0
                    target_pose.pose.orientation.w = 1.0
                
                # Make sure we found at least one object before setting the target ID
                if the_object != None:
                    target_id = "object%d"%the_object
                    
            # Insert the support surface into the planning scene
            for obj in find_result.support_surfaces:
                # Extend surface to floor
                height = obj.primitive_poses[0].position.z
                obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
                                                2.0, # make table wider
                                                obj.primitives[0].dimensions[2] + height]
                obj.primitive_poses[0].position.z += -height/2.0
    
                # Add to scene
                scene.addSolidPrimitive(obj.name,
                                        obj.primitives[0],
                                        obj.primitive_poses[0],
                                        wait = False)
                
                # Get the table dimensions
                table_size = obj.primitives[0].dimensions
                
            # If no objects detected, try again
            if the_object == None or target_size is None:
                rospy.logerr("Nothing to grasp! try again...")
                continue
    
            # Wait for the scene to sync
            scene.waitForSync()
    
            # Set colors of the table and the object we are grabbing
            scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0)  # orange
            scene.setColor(find_result.objects[the_object].object.support_surface, 0.3, 0.3, 0.3, 0.7)  # grey
            scene.sendColors()
    
            # Skip pick-and-place if we are just detecting objects
            if args.objects:
                if args.once:
                    exit(0)
                else:
                    continue
    
            # Get the support surface ID
            support_surface = find_result.objects[the_object].object.support_surface
                        
            # Set the support surface name to the table object
            right_arm.set_support_surface_name(support_surface)
            
            # Specify a pose to place the target after being picked up
            place_pose = PoseStamped()
            place_pose.header.frame_id = REFERENCE_FRAME
            place_pose.pose.position.x = target_pose.pose.position.x
            place_pose.pose.position.y = 0.03
            place_pose.pose.position.z = table_size[2] + target_size[2] / 2.0 + 0.015
            place_pose.pose.orientation.w = 1.0
                            
            # Initialize the grasp pose to the target pose
            grasp_pose = target_pose
             
            # Shift the grasp pose half the size of the target to center it in the gripper
            try:
                grasp_pose.pose.position.x += target_size[0] / 2.0
                grasp_pose.pose.position.y -= 0.01
                grasp_pose.pose.position.z += target_size[2] / 2.0
            except:
                rospy.loginfo("Invalid object size so skipping")
                continue

            # Generate a list of grasps
            grasps = self.make_grasps(grasp_pose, [target_id])
     
            # Publish the grasp poses so they can be viewed in RViz
            for grasp in grasps:
                self.gripper_pose_pub.publish(grasp.grasp_pose)
                rospy.sleep(0.2)
         
            # Track success/failure and number of attempts for pick operation
            result = None
            n_attempts = 0
            
            # Set the start state to the current state
            right_arm.set_start_state_to_current_state()
             
            # Repeat until we succeed or run out of attempts
            while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
                result = right_arm.pick(target_id, grasps)
                n_attempts += 1
                rospy.loginfo("Pick attempt: " +  str(n_attempts))
                rospy.sleep(1.0)
             
            # If the pick was successful, attempt the place operation   
            if result == MoveItErrorCodes.SUCCESS:
                result = None
                n_attempts = 0
                  
                # Generate valid place poses
                places = self.make_places(place_pose)
                
                # Set the start state to the current state
                #right_arm.set_start_state_to_current_state()
                  
                # Repeat until we succeed or run out of attempts
                while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
                    for place in places:
                        result = right_arm.place(target_id, place)
                        if result == MoveItErrorCodes.SUCCESS:
                            break
                    n_attempts += 1
                    rospy.loginfo("Place attempt: " +  str(n_attempts))
                    rospy.sleep(0.2)
                      
                if result != MoveItErrorCodes.SUCCESS:
                    rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
            else:
                rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")

            rospy.sleep(2)
            
            # Open the gripper to the neutral position
            right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
            right_gripper.go()
            
            rospy.sleep(2)
            
            # Return the arm to the "resting" pose stored in the SRDF file
            right_arm.set_named_target('resting')
            right_arm.go()
             
            rospy.sleep(2)
            
            # Give the servos a rest
            arbotix_relax_all_servos()
            
            rospy.sleep(2)
        
            if args.once:
                # Shut down MoveIt cleanly
                moveit_commander.roscpp_shutdown()
                
                # Exit the script
                moveit_commander.os._exit(0)
Ejemplo n.º 20
0
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

import argparse
import rospy
from moveit_python import PlanningSceneInterface

if __name__ == "__main__":
    parser = argparse.ArgumentParser(
        description="Attach objects to link in the MoveIt planning scene.")
    parser.add_argument("name", help="Name of the box to be attached")
    parser.add_argument("link", help="Name of link to where the box should be attached")
    parser.add_argument("x", type=float, help="X coordinate of center of box")
    parser.add_argument("y", type=float, help="Y coordinate of center of box")
    parser.add_argument("z", type=float, help="Z coordinate of center of box")
    parser.add_argument("size_x", type=float, help="Size of box in x dimension")
    parser.add_argument("size_y", type=float, help="Size of box in y dimension")
    parser.add_argument("size_z", type=float, help="Size of box in z dimension")
    args = parser.parse_args()

    if args.name:
      if args.link:
        rospy.init_node("attach_box")
        scene = PlanningSceneInterface("/base_link")
        print("Attach Object with name: %s to Link: %s" % (args.name, args.link))
        scene.attachBox(args.name, args.size_x, args.size_y, args.size_z,
                        args.x, args.y, args.z, args.link)
    else:
        parser.print_help()

Ejemplo n.º 21
0
class PickPlaceDemo():
    def __init__(self):
        self._scene = PlanningSceneInterface('base_link')
        self._pickplace = PickPlaceInterface('xarm5', 'gripper', verbose=True)
        self._move_group = MoveGroupInterface('xarm5', 'base_link')

    def setup_scene(self):
        # remove previous objects
        for name in self._scene.getKnownCollisionObjects():
            self._scene.removeCollisionObject(name, False)
        for name in self._scene.getKnownAttachedObjects():
            self._scene.removeAttachedObject(name, False)
        self._scene.waitForSync()

        self.__addBoxWithOrientation('box',
                                     0.25,
                                     0.25,
                                     0.25,
                                     -0.45,
                                     0.1,
                                     0.125,
                                     0,
                                     0,
                                     np.radians(45),
                                     wait=False)
        self._scene.waitForSync()

    def __addBoxWithOrientation(self,
                                name,
                                size_x,
                                size_y,
                                size_z,
                                x,
                                y,
                                z,
                                roll,
                                pitch,
                                yaw,
                                wait=True):
        s = SolidPrimitive()
        s.dimensions = [size_x, size_y, size_z]
        s.type = s.BOX

        ps = PoseStamped()
        ps.header.frame_id = self._scene._fixed_frame
        ps.pose.position.x = x
        ps.pose.position.y = y
        ps.pose.position.z = z
        q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
        ps.pose.orientation.x = q[0]
        ps.pose.orientation.y = q[1]
        ps.pose.orientation.z = q[2]
        ps.pose.orientation.w = q[3]

        self._scene.addSolidPrimitive(name, s, ps.pose, wait)

    def pickup(self):
        g = Grasp()
        self._pickplace.pickup("box", [g], support_name="box")
Ejemplo n.º 22
0
def picknplace():
    # Define initial parameters
    p = PlanningSceneInterface("base")
    g = MoveGroupInterface("both_arms", "base")
    gr = MoveGroupInterface("right_arm", "base")
    gl = MoveGroupInterface("left_arm", "base")
    leftgripper = baxter_interface.Gripper('left')
    leftgripper.calibrate()
    leftgripper.open()
    jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
    jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
    jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2']
    pos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
    pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
    lpos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519]
    lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384]    
    rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
    rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
    
    # Clear planning scene
    p.clear()
    # Add table as attached object
    p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])

    # Move both arms to start state              
    g.moveToJointPosition(jts_both, pos1, plan_only=False)
    
    # Get obj locations
    temp = rospy.wait_for_message("Dpos", PoseArray)
    locs = temp.poses 
    locs_x = []
    locs_y = []
    orien = []
    size = []

    for i in range(len(locs)):
        locs_x.append(0.57 + locs[i].position.x)
        locs_y.append(-0.011 + locs[i].position.y)
        orien.append(locs[i].position.z*pi/180)
        size.append(locs[i].orientation.x)

    # Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
    ind_rmv = []
    for i in range(0,len(locs)):
        if (locs_y[i] > 0.42):
            ind_rmv.append(i)
            continue
        for j in range(i,len(locs)):
            if not (i == j):
                if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
                    ind_rmv.append(i)
    
    locs_x = del_meth(locs_x, ind_rmv)
    locs_y = del_meth(locs_y, ind_rmv)
    orien = del_meth(orien, ind_rmv) 
    size = del_meth(size, ind_rmv)

    # Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
    if locs_x:  
        ig0 = itemgetter(0)
        sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))

        locs_x = list(sorted_lists[1])
        locs_y = list(sorted_lists[2])
        orien = list(sorted_lists[3])
        size = list(sorted_lists[0])

    # Loop to continue pick and place until all objects are cleared from table
    j=0
    k=0
    while locs_x:
        p.clear() # CLear planning scene of all collision objects

        # Attach table as attached collision object to base of baxter
        p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])
        xn = locs_x[0]
        yn = locs_y[0]
        zn = -0.06
        thn = orien[0]
        sz = size[0]
        if thn > pi/4:
            thn = -1*(thn%(pi/4))

        # Add collision objects into planning scene
        objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']
        for i in range(1,len(locs_x)):
            p.addCube(objlist[i], 0.05, locs_x[i], locs_y[i], -0.05)
        p.waitForSync()        

        # Move both arms to pos2 (Right arm away and left arm on table)
        g.moveToJointPosition(jts_both, pos2, plan_only=False)

        # Move left arm to pick object and pick object
        pickgoal = PoseStamped() 
        pickgoal.header.frame_id = "base"
        pickgoal.header.stamp = rospy.Time.now()
        pickgoal.pose.position.x = xn
        pickgoal.pose.position.y = yn
        pickgoal.pose.position.z = zn+0.1
        pickgoal.pose.orientation.x = 1.0
        pickgoal.pose.orientation.y = 0.0
        pickgoal.pose.orientation.z = 0.0
        pickgoal.pose.orientation.w = 0.0
        gl.moveToPose(pickgoal, "left_gripper", plan_only=False)

        # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles 
        pickgoal.pose = rotate_pose_msg_by_euler_angles(pickgoal.pose, 0.0, 0.0, thn)
        gl.moveToPose(pickgoal, "left_gripper", plan_only=False) 

        pickgoal.pose.position.z = zn
        gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 0.14, plan_only=False)
        leftgripper.close()

        pickgoal.pose.position.z = zn+0.1
        gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
        
        # Move both arms to pos1
        g.moveToJointPosition(jts_both, pos1, plan_only=False)

        # Get obj locations
        temp = rospy.wait_for_message("Dpos", PoseArray)
        locs = temp.poses 
        locs_x = []
        locs_y = []
        orien = []
        size = []

        for i in range(len(locs)):
            locs_x.append(0.57 + locs[i].position.x)
            locs_y.append(-0.011 + locs[i].position.y)
            orien.append(locs[i].position.z*pi/180)
            size.append(locs[i].orientation.x)

        # Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
        ind_rmv = []
        for i in range(0,len(locs)):
            if (locs_y[i] > 0.42):
                ind_rmv.append(i)
                continue
            for j in range(i,len(locs)):
                if not (i == j):
                    if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
                        ind_rmv.append(i)
        
        locs_x = del_meth(locs_x, ind_rmv)
        locs_y = del_meth(locs_y, ind_rmv)
        orien = del_meth(orien, ind_rmv) 
        size = del_meth(size, ind_rmv)

        # Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
        if locs_x:  
            ig0 = itemgetter(0)
            sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))

            locs_x = list(sorted_lists[1])
            locs_y = list(sorted_lists[2])
            orien = list(sorted_lists[3])
            size = list(sorted_lists[0])

        # Place object
        stleft = PoseStamped() 
        stleft.header.frame_id = "base"
        stleft.header.stamp = rospy.Time.now()
        stleft.pose.position.x = 0.65
        stleft.pose.position.y = 0.55
        stleft.pose.position.z = 0.1
        stleft.pose.orientation.x = 1.0
        stleft.pose.orientation.y = 0.0
        stleft.pose.orientation.z = 0.0
        stleft.pose.orientation.w = 0.0

        # Stack objects (large cubes) if size if greater than some threshold
        if sz > 16.:            
            stleft.pose.position.x = 0.65
            stleft.pose.position.y = 0.7
            stleft.pose.position.z = -0.04+(k*0.05)
            k += 1
        
        gl.moveToPose(stleft, "left_gripper", plan_only=False)        
        leftgripper.open()

        stleft.pose.position.z = 0.3
        gl.moveToPose(stleft, "left_gripper", plan_only=False)
Ejemplo n.º 23
0
def dyn_wedge():

	pub = rospy.Publisher("finished", String, queue_size=10)

	# Initialize MoveIt scene
	p = PlanningSceneInterface("base")
	arms_group = MoveGroupInterface("both_arms", "base")
	rightarm_group = MoveGroupInterface("right_arm", "base")
	leftarm_group = MoveGroupInterface("left_arm", "base")
	
	# Create right arm instance
	right_arm = baxter_interface.limb.Limb('right')
	
	# Create right gripper instance
	right_gripper = baxter_interface.Gripper('right')
	right_gripper.calibrate()
	right_gripper.open()
	
	right_gripper.close()

	offset_zero_point=0.903
	table_size_x = 0.714655654394
	table_size_y = 1.05043717328
	table_size_z = 0.729766045265
	center_x = 0.457327827197
	center_y = 0.145765166941
	center_z = -0.538116977368	 
	table_distance_from_gripper = -offset_zero_point+table_size_z+0.0275/2
	j=0
	k=0

	# Initialize object list
	objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']
	p.clear()
	p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal'])
	p.waitForSync()
	# Move both arms to start state.

	# Initial pose
	rpos = PoseStamped()
	rpos.header.frame_id = "base"
	rpos.header.stamp = rospy.Time.now()
	rpos.pose.position.x = 0.555
	rpos.pose.position.y = 0.0
	rpos.pose.position.z = 0.206 #0.18
	rpos.pose.orientation.x = 1.0
	rpos.pose.orientation.y = 0.0
	rpos.pose.orientation.z = 0.0
	rpos.pose.orientation.w = 0.0

   	lpos = PoseStamped()
	lpos.header.frame_id = "base"
	lpos.header.stamp = rospy.Time.now()
	lpos.pose.position.x = 0.555
	lpos.pose.position.y = 0.65
	lpos.pose.position.z = 0.206#0.35
	lpos.pose.orientation.x = 1.0
	lpos.pose.orientation.y = 0.0
	lpos.pose.orientation.z = 0.0
	lpos.pose.orientation.w = 0.0

	rightarm_group.moveToPose(rpos, "right_gripper", max_velocity_scaling_factor=1, plan_only=False)
	leftarm_group.moveToPose(lpos, "left_gripper", max_velocity_scaling_factor=1, plan_only=False)

	# Get the middle point between the two centroids
	locs = rospy.wait_for_message("clasificacion", PoseArray)

	if(len(locs.poses)!=0):

		punto_medio1 = PoseStamped()
		punto_medio1.header.frame_id = "base"
		punto_medio1.header.stamp = rospy.Time.now()
		punto_medio1.pose.position.x = locs.poses[0].position.x
		punto_medio1.pose.position.y = locs.poses[0].position.y
		punto_medio1.pose.position.z = -0.08
		punto_medio1.pose.orientation.x = locs.poses[0].orientation.x
		punto_medio1.pose.orientation.y = locs.poses[0].orientation.y
		punto_medio1.pose.orientation.z = locs.poses[0].orientation.z
		punto_medio1.pose.orientation.w = locs.poses[0].orientation.w

	else:
		sys.exit("No se encuentran los puntos")

	# Get the middle point again after a few seconds
	tiempo = 3
	time.sleep(tiempo)
	locs = rospy.wait_for_message("clasificacion", PoseArray)

	if(len(locs.poses)!=0):

		punto_medio2 = PoseStamped()
		punto_medio2.header.frame_id = "base"
		punto_medio2.header.stamp = rospy.Time.now()
		punto_medio2.pose.position.x = locs.poses[0].position.x
		punto_medio2.pose.position.y = locs.poses[0].position.y
		punto_medio2.pose.position.z = -0.08
		punto_medio2.pose.orientation.x = locs.poses[0].orientation.x
		punto_medio2.pose.orientation.y = locs.poses[0].orientation.y
		punto_medio2.pose.orientation.z = locs.poses[0].orientation.z
		punto_medio2.pose.orientation.w = locs.poses[0].orientation.w

	else:
		sys.exit("No se encuentran los puntos")

	# Calculate speed of objects
	vel = (punto_medio2.pose.position.y - punto_medio1.pose.position.y) / tiempo

	# Predict position within a certain time
	nuevo_tiempo = 2.1
	posicion = nuevo_tiempo * vel * 2

	if(posicion>=0):
		nueva_y = punto_medio2.pose.position.y - posicion
	else:
		nueva_y = punto_medio2.pose.position.y + posicion

	orientacion = (-1) * 1/punto_medio2.pose.orientation.x

	new_or = 0.26

	if orientacion > 0:
		new_or = -0.26

	# If there is time enough to sweep the objects
	if vel > -0.08:
		start = time.time()
		punto_medio2.pose.position.y = nueva_y - nueva_y/2
		punto_medio2.pose.position.x = punto_medio1.pose.position.x - 0.03
		punto_medio2.pose = rotate_pose_msg_by_euler_angles(punto_medio2.pose, 0.0, 0.0, orientacion - new_or)
		rightarm_group.moveToPose(punto_medio2, "right_gripper", max_velocity_scaling_factor=1, plan_only=False)
		end = time.time()

		tiempo = end - start
		while(nuevo_tiempo - tiempo >= 1):
			end = time.time()
			nuevo_tiempo = end - start
	else:
		punto_medio2.pose.position.y = nueva_y

	print(punto_medio2.pose.position.y)

	punto_medio2.pose.position.x = punto_medio1.pose.position.x
	punto_medio2.pose.position.y += 0.05 
	punto_medio2.pose = rotate_pose_msg_by_euler_angles(punto_medio2.pose, 0.0, 0.0, orientacion)
	rightarm_group.moveToPose(punto_medio2, "right_gripper", max_velocity_scaling_factor=1, plan_only=False)
	obj = punto_medio2.pose.position.y + 0.16
	neg = 1

	while punto_medio2.pose.position.y < obj:
		punto_medio2.pose.position.y += 0.03
		# Shake arm
		punto_medio2.pose.position.x += neg * 0.09
		rightarm_group.moveToPose(punto_medio2, "right_gripper", max_velocity_scaling_factor=1, plan_only=False)
		neg = (-1)*neg

	time.sleep(2)

	rightarm_group.moveToPose(rpos, "right_gripper", max_velocity_scaling_factor=1, plan_only=False)

	freemem = "free"
	pub.publish(freemem)
Ejemplo n.º 24
0
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = 'base_link'

# Creat planning scene and move group to interact with robot
from moveit_msgs.msg import MoveItErrorCodes
from moveit_python import MoveGroupInterface, PlanningSceneInterface

# rospy.init_node("move_torso_up")

# Create move group interface for a fetch robot
move_group = MoveGroupInterface("arm_with_torso", "base_link")

# Define ground plane
# This creates objects in the planning scene that mimic the ground
# If these were not in place gripper could hit the ground
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# Add table + object --> Hard coded, ! Have to lift arm beforehand, else stuck
planning_scene.removeCollisionObject("table")
planning_scene.addCube("table", 2, 1.1, 0.0, 0.82+0.1-1)

# Execute interpolated poses
for pose in approach_poses:
    # Finish building the Pose_stamped message
class GraspingClient(object):

    def __init__(self):
        self.scene = PlanningSceneInterface("base_link")
        self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
        self.move_group = MoveGroupInterface("arm", "base_link")

        find_topic = "basic_grasping_perception/find_objects"
        rospy.loginfo("Waiting for %s..." % find_topic)
        self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
        self.find_client.wait_for_server()
        obj_cts = 0

    def updateScene(self):
        # find objects
        goal = FindGraspableObjectsGoal()
        goal.plan_grasps = True
        print " find goal"
        self.find_client.send_goal(goal)
        self.find_client.wait_for_result(rospy.Duration(5.0))
        print " find client"
        find_result = self.find_client.get_result()

        # remove previous objects
        for name in self.scene.getKnownCollisionObjects():
            print "the object %s should be removed" %(name)
            self.scene.removeCollisionObject(name, False)
        for name in self.scene.getKnownAttachedObjects():
            print "the object %s should be removed" %(name)
            self.scene.removeAttachedObject(name, False)
        self.scene.waitForSync()

        # insert objects to scene
        objects = list()
        idx = -1
        for obj in find_result.objects:
            idx += 1
            obj.object.name = "object%d"%idx
            print "-----------------------"
            print obj.object.primitive_poses[0]
            print " x: %d" %(obj.object.primitive_poses[0].position.x)
            # if obj.object.primitive_poses[0].position.y > 0.0
            self.scene.addSolidPrimitive(obj.object.name,
                                         obj.object.primitives[0],
                                         obj.object.primitive_poses[0],
                                         use_service = False)
            # def 	addSolidPrimitive (self, name, solid, pose, use_service=True)
            print "1, %d" %(idx)
            if obj.object.primitive_poses[0].position.x < 1.25:
                objects.append([obj, obj.object.primitive_poses[0].position.z])

        for obj in find_result.support_surfaces:
            # extend surface to floor, and make wider since we have narrow field of view
            print "2,"
            height = obj.primitive_poses[0].position.z
            obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
                                            1.5,  # wider
                                            obj.primitives[0].dimensions[2] + height]
            obj.primitive_poses[0].position.z += -height/2.0

            # add to scene
            self.scene.addSolidPrimitive(obj.name,
                                         obj.primitives[0],
                                         obj.primitive_poses[0],
                                         use_service = True
                                         )

        self.scene.waitForSync()

        # store for grasping
        #self.objects = find_result.objects
        self.surfaces = find_result.support_surfaces

        # store graspable objects by Z
        objects.sort(key=lambda object: object[1])
        objects.reverse()
        self.objects = [object[0] for object in objects]
        rospy.loginfo("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
        rospy.loginfo("number of objects...:::::::" + str(len(objects)))
        #for object in objects:
        #    print(object[0].object.name, object[1])
        #exit(-1)

    def getGraspableObject(self):
        graspable = None
        for obj in self.objects:
            # need grasps
            if len(obj.grasps) < 1:
                rospy.loginfo("must more than one object")
                continue
            # check size
            if obj.object.primitives[0].dimensions[0] < 0.03 or \
               obj.object.primitives[0].dimensions[0] > 0.25 or \
               obj.object.primitives[0].dimensions[0] < 0.03 or \
               obj.object.primitives[0].dimensions[0] > 0.25 or \
               obj.object.primitives[0].dimensions[0] < 0.03 or \
               obj.object.primitives[0].dimensions[0] > 0.25:
                continue
            # has to located in +y 
            if obj.object.primitive_poses[0].position.y < 0.0:
                print "has to located in +y"
                continue
            # has to be on table
            if obj.object.primitive_poses[0].position.z < 0.5:
                print "z has to larger than 0.5 "
                continue
            rospy.loginfo("object pose:")
            print obj.object.primitive_poses[0], obj.object.primitives[0]
            return obj.object, obj.grasps
        # nothing detected
        return None, None

    def getSupportSurface(self, name):
        for surface in self.support_surfaces:
            if surface.name == name:
                return surface
        return None

    def getPlaceLocation(self):
        pass

    def pick(self, block, grasps):
        success, pick_result = self.pickplace.pick_with_retry(block.name,
                                                              grasps,
                                                              support_name=block.support_surface,
                                                              scene=self.scene)
        self.pick_result = pick_result
        return success

    def place(self, block, pose_stamped):
        places = list()
        l = PlaceLocation()
        l.place_pose.pose = pose_stamped.pose
        l.place_pose.header.frame_id = pose_stamped.header.frame_id

        # copy the posture, approach and retreat from the grasp used
        l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
        l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
        l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
        places.append(copy.deepcopy(l))
        # create another several places, rotate each by 360/m degrees in yaw direction
        m = 16 # number of possible place poses
        pi = 3.141592653589
        for i in range(0, m-1):
            l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m)
            places.append(copy.deepcopy(l))

        success, place_result = self.pickplace.place_with_retry(block.name,
                                                                places,
                                                                scene=self.scene)
        return success

    def tuck(self):
        joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
                  "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
        pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
        while not rospy.is_shutdown():
            result = self.move_group.moveToJointPosition(joints, pose, 0.02)
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                return

    def stow(self):
        joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
                  "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
        pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
        while not rospy.is_shutdown():
            result = self.move_group.moveToJointPosition(joints, pose, 0.02)
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                return

    def intermediate_stow(self):
        joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
                  "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
        pose = [0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0]
        while not rospy.is_shutdown():
            result = self.move_group.moveToJointPosition(joints, pose, 0.02)
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                return
Ejemplo n.º 26
0
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

import rospy
from moveit_python import PlanningSceneInterface

if __name__ == "__main__":
    rospy.init_node("list_objects")
    scene = PlanningSceneInterface("base_link")
    for name in scene.getKnownCollisionObjects():
        print(name)
    for name in scene.getKnownAttachedObjects():
        print(name)
    if len(scene.getKnownCollisionObjects() + scene.getKnownAttachedObjects()) == 0:
        print("No objects in planning scene.")

Ejemplo n.º 27
0
class World:
    def __init__(self, tower_array, disk_array, config, debug=0):
        print "[INFO] Initialise World"
        self.DEBUG = debug
        # initialise arrays
        self.tower_array = tower_array
        self.disk_array = disk_array
        # configs
        self.max_gripper = config.get_max_gripper()
        self.disk_height = config.disk_height
        self.tower_positions = config.tower_positions

        self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME)
        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)

    """Calls a method to display the collision objects.
    """
    def create_planning_scene(self):
        print "[INFO] Create_planning_scene"
        self.display_towers_and_disks()

    """This method collects all needed information and
    publish them to other methods.
    """
    def display_towers_and_disks(self):
        print "[INFO] Display towers and disks"
        for tower in self.tower_array:
            # call method to publish new tower
            self.publish_scene(tower.get_position(), tower)
            # set color by name
            self.planning_scene_interface.setColor(tower.get_name(), 1.0, 1.0, 1.0)
            # publish color
            self.planning_scene_interface.sendColors()
        for disk in self.disk_array:
            self.publish_scene(disk.get_position(), None, disk)
            self.planning_scene_interface.setColor(disk.get_id(), disk.get_color()[0], disk.get_color()[1],
                                                   disk.get_color()[2], disk.get_color()[3])
            self.planning_scene_interface.sendColors()
        # wait for sync after publishing collision objects
        self.planning_scene_interface.waitForSync(5.0)
        rospy.sleep(5)

    """This method creates a box or a cylinder with methods of
    the planning scene interface.
    :param position: the position of the new collision object
    :param tower: the tower object
    :param disk: the disk object
    """
    def publish_scene(self, position, tower=None, disk=None):
        if tower is not None:
            self.planning_scene_interface.addBox(tower.get_name(), self.max_gripper / 100.0, self.max_gripper / 100.0,
                                                 (self.tower_positions[tower.get_id() - 1][2] * 2), position[0],
                                                 position[1], position[2])
        else:
            self.planning_scene_interface.addCylinder(disk.get_id(), self.disk_height, disk.get_diameter() / 2,
                                                      position[0], position[1], position[2])

    """This method cleans the planning scene.
    :param close: with this flag a new planning scene objects will be removed
    in sync otherwise the object will be deleted without syncing the scene
    """
    def clean_up(self, close=False):
        if close:
            # get the actual list after game
            self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME)
        for name in self.planning_scene_interface.getKnownCollisionObjects():
            if self.DEBUG is 1:
                print("[DEBUG] Removing object %s" % name)
            self.planning_scene_interface.removeCollisionObject(name, False)
        for name in self.planning_scene_interface.getKnownAttachedObjects():
            if self.DEBUG is 1:
                print("[DEBUG] Removing attached object %s" % name)
            self.planning_scene_interface.removeAttachedObject(name, False)
        if close:
            self.planning_scene_interface.waitForSync(5.0)
            pass

    """This method corrects the position of the moved disk.
    :param tower: parent tower of the disk
    """
    def refresh_disk_pose(self, tower):
        print "[INFO] Refresh disk pose"
        disk = tower.get_last()
        if self.DEBUG is 1:
            print "[DEBUG] Refresh", disk.get_id(), "pose:", disk.get_position(), "tower size", tower.get_size(),\
                "tower pose", tower.get_position()
        # remove the disk from planning scene
        self.planning_scene_interface.removeCollisionObject(disk.get_id(), False)
        # publish collision object and set old color
        self.publish_scene(disk.get_position(), None, disk)
        self.planning_scene_interface.setColor(disk.get_id(), disk.get_color()[0], disk.get_color()[1],
                                               disk.get_color()[2], disk.get_color()[3])
        self.planning_scene_interface.sendColors()
Ejemplo n.º 28
0
def setBoundaries():
    '''
    This is a fix for the FETCH colliding with itself
    Define ground plane
    This creates objects in the planning scene that mimic the ground
    If these were not in place gripper could hit the ground
    '''
    planning_scene = PlanningSceneInterface("base_link")
    planning_scene.removeCollisionObject("my_front_ground")
    planning_scene.removeCollisionObject("my_back_ground")
    planning_scene.removeCollisionObject("my_right_ground")
    planning_scene.removeCollisionObject("my_left_ground")
    planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
    planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
    planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
    planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
Ejemplo n.º 29
0
class GraspingClient(object):
	
	def __init__(self):
		self.scene = PlanningSceneInterface("base_link")
		self.pickplace = PickPlaceInterface("left_arm", "left_hand", verbose = True)
		self.move_group = MoveGroupInterface("left_arm", "base_link")

		find_topic = "basic_grasping_perception/find_objects"
		rospy.loginfo("Waiting for %s..." % find_topic)
		self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
		self.find_client.wait_for_server(rospy.Duration(15.0))
		self.cubes = []
		self.tf_listener = TransformListener()
		# self.gripper_client = actionlib.SimpleActionClient("/robot/end_effector/left_gripper/gripper_action", GripperCommandAction)
		# self.gripper_client.wait_for_server()
		# rospy.loginfo("...connected.")
		rospy.sleep(2.0)
	
	def updateScene(self, remove_collision = False):
		if remove_collision:
			for name in self.scene.getKnownCollisionObjects():
				self.scene.removeCollisionObject(name, False)
			for name in self.scene.getKnownAttachedObjects():
				self.scene.removeAttachedObject(name, False)
			self.scene.waitForSync()
			return
		
		# find objects
		self.cubes = []
		goal = FindGraspableObjectsGoal()
		goal.plan_grasps = True
		self.find_client.send_goal(goal)
		self.find_client.wait_for_result(rospy.Duration(15.0))
		find_result = self.find_client.get_result()
		# rospy.loginfo(find_result)
		
		# remove previous objects
		for name in self.scene.getKnownCollisionObjects():
			self.scene.removeCollisionObject(name, False)
		for name in self.scene.getKnownAttachedObjects():
			self.scene.removeAttachedObject(name, False)
		self.scene.waitForSync()

		# insert objects to scene
		idx = -1
		print(find_result.objects)
		# print(find_result.support_surfaces)
		# for obj in find_result.objects:
		# 	idx += 1
		# 	print(idx)
		# 	obj.object.name = "object%d" % idx
		# 	self.scene.addSolidPrimitive(obj.object.name,
		# 	                             obj.object.primitives[0],
		# 	                             obj.object.primitive_poses[0],
		# 	                             wait = False)
		# 	self.cubes.append(obj.object.primitive_poses[0])
		#
		# for obj in find_result.support_surfaces:
		# 	# extend surface to floor, and make wider since we have narrow field of view
		# 	height = obj.primitive_poses[0].position.z
		# 	obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
		# 	                                1.5,  # wider
		# 	                                obj.primitives[0].dimensions[2] + height]
		# 	obj.primitive_poses[0].position.z += -height / 2.0
		#
		# 	# add to scene
		# 	self.scene.addSolidPrimitive(obj.name,
		# 	                             obj.primitives[0],
		# 	                             obj.primitive_poses[0],
		# 	                             wait = False)
		#
		# self.scene.waitForSync()
		#
		# # store for grasping
		# self.objects = find_result.objects
		# self.surfaces = find_result.support_surfaces

	def getGraspableCube(self):
		graspable = None
		for obj in self.objects:
			# need grasps
			if len(obj.grasps) < 1:
				continue
			# check size
			if obj.object.primitives[0].dimensions[0] < 0.05 or \
					obj.object.primitives[0].dimensions[0] > 0.07 or \
					obj.object.primitives[0].dimensions[0] < 0.05 or \
					obj.object.primitives[0].dimensions[0] > 0.07 or \
					obj.object.primitives[0].dimensions[0] < 0.05 or \
					obj.object.primitives[0].dimensions[0] > 0.07:
				continue
			# has to be on table
			if obj.object.primitive_poses[0].position.z < 0.5:
				continue
			return obj.object, obj.grasps
		# nothing detected
		return None, None
	
	def get_sequence(self, seq_list):
		start = [self.objects[seq_list[0]].object, self.objects[seq_list[0]].grasps] or [None, None]
		
		target = [self.objects[seq_list[1]].object, self.objects[seq_list[1]].grasps] or [None, None]
		
		res = {'start': start, 'target': target}
		
		return res
	
	def get_transform_pose(self, pose):
		point = PoseStamped()
		point.header.frame_id = 'base_link'
		point.header.stamp = rospy.Time()
		point.pose = pose
		
		return self.tf_listener.transformPose('/map', point).pose.position
	
	def getPlaceLocation(self):
		pass
	
	def pick(self, block, grasps):
		success, pick_result = self.pickplace.pick_with_retry(block.name,
		                                                      grasps,
		                                                      retries = 5,
		                                                      support_name = block.support_surface,
		                                                      scene = self.scene)
		self.pick_result = pick_result
		return success
	
	def place(self, block, pose_stamped):
		places = list()
		l = PlaceLocation()
		l.place_pose.pose = pose_stamped.pose
		l.place_pose.header.frame_id = pose_stamped.header.frame_id
		
		# copy the posture, approach and retreat from the grasp used
		l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
		l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
		l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
		places.append(copy.deepcopy(l))
		# create another several places, rotate each by 360/m degrees in yaw direction
		m = 16  # number of possible place poses
		pi = 3.141592653589
		for i in range(0, m - 1):
			l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m)
			places.append(copy.deepcopy(l))
		
		success, place_result = self.pickplace.place_with_retry(block.name,
		                                                        places,
		                                                        scene = self.scene,
		                                                        retries = 5)
		# print(success)
		return success
	
	def tuck(self):
		joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
		          "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
		pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
		# pose =[ 1.58, -0.056, -0.01, -1.31, -0.178, -0.13, 0.16]
		gripper_goal = GripperCommandGoal()
		gripper_goal.command.max_effort = 0.0
		gripper_goal.command.position = 0.09
		self.gripper_client.send_goal(gripper_goal)
		
		self.gripper_client.wait_for_result(rospy.Duration(5))
		start = rospy.Time.now()
		while rospy.Time.now() - start <= rospy.Duration(10.0):  # rospy.is_shutdown():
			result = self.move_group.moveToJointPosition(joints, pose, 0.02)
			if result.error_code.val == MoveItErrorCodes.SUCCESS:
				return
		return
	
	def gripper_opening(self, opening = 0.09):
		gripper_goal = GripperCommandGoal()
		gripper_goal.command.max_effort = 0.0
		gripper_goal.command.position = opening
		self.gripper_client.send_goal(gripper_goal)
		self.gripper_client.wait_for_result(rospy.Duration(5))
Ejemplo n.º 30
0
def move():
	rospy.init_node("move")

	move_group = MoveGroupInterface("arm_with_torso", "base_link")


	#planning scene setup
	planning_scene = PlanningSceneInterface("base_link")
	planning_scene.removeCollisionObject("my_front_ground")
	planning_scene.removeCollisionObject("my_back_ground")
	planning_scene.removeCollisionObject("my_right_ground")
	planning_scene.removeCollisionObject("my_left_ground")
	planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
	planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
	planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
	planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)


	#move head camera
	client = actionlib.SimpleActionClient("head_controller/point_head", PointHeadAction)
	rospy.loginfo("Waiting for head_controller...")
	client.wait_for_server()

	goal = PointHeadGoal()
	goal.target.header.stamp = rospy.Time.now()
	goal.target.header.frame_id = "base_link"
	goal.target.point.x = 1.2
	goal.target.point.y = 0.0
	goal.target.point.z = 0.0
	goal.min_duration = rospy.Duration(1.0)
	client.send_goal(goal)
	client.wait_for_result()

	#arm setup
	move_group = MoveGroupInterface("arm", "base_link")
	#set arm initial position
	joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
                  "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
	pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
	while not rospy.is_shutdown():
		result = move_group.moveToJointPosition(joints, pose, 0.02)
		if result.error_code.val == MoveItErrorCodes.SUCCESS:
			return
Ejemplo n.º 31
0
    def __init__(self):

        self.moving_mode = False
        self.waldo_mode = False
        self.prev_joy_buttons = 0

        # See http://docs.fetchrobotics.com/robot_hardware.html#naming-conventions
        self.joint_names = [
            #'torso_lift_joint',
            #'bellows_joint',
            #'head_pan_joint',
            #'head_tilt_joint',
            'shoulder_pan_joint',
            'shoulder_lift_joint',
            'upperarm_roll_joint',
            'elbow_flex_joint',
            'forearm_roll_joint',
            'wrist_flex_joint',
            'wrist_roll_joint',
            'l_gripper_finger_joint',
            #'r_gripper_finger_joint',
        ]
        self.joint_name_map = dict([
            (name, index) for index, name in enumerate(self.joint_names)
        ])
        self.cur_joint_pos = np.zeros([len(self.joint_names)])
        self.cur_joint_vel = np.zeros([len(self.joint_names)])
        self.cur_joint_effort = np.zeros([len(self.joint_names)])

        self.cur_base_scan = np.zeros([662], dtype=np.float32)
        self.cur_head_camera_depth_image = np.zeros([480, 640],
                                                    dtype=np.float32)
        self.cur_head_camera_rgb_image = np.zeros([480, 640, 3],
                                                  dtype=np.uint8)

        self.timeseq = None
        self.timeseq_mutex = threading.Lock()
        self.image_queue = Queue.Queue()
        self.image_compress_thread = threading.Thread(
            target=self.image_compress_main)
        self.image_compress_thread.daemon = True
        self.image_compress_thread.start()

        logger.info('Subscribing...')
        self.subs = []
        if 1:
            self.subs.append(
                rospy.Subscriber('/joint_states', JointState,
                                 self.joint_states_cb))
        if 0:
            self.subs.append(
                rospy.Subscriber('/base_scan', LaserScan, self.base_scan_cb))
        if 1:
            self.subs.append(
                rospy.Subscriber('/head_camera/depth/image', numpy_msg(Image),
                                 self.head_camera_depth_image_cb))
        if 1:
            self.subs.append(
                rospy.Subscriber('/head_camera/rgb/image_raw',
                                 numpy_msg(Image),
                                 self.head_camera_rgb_image_raw_cb))
        if 1:
            self.subs.append(
                rospy.Subscriber('/spacenav/joy', Joy, self.spacenav_joy_cb))
        logger.info('Subscribed')

        self.arm_effort_pub = rospy.Publisher(
            '/arm_controller/weightless_torque/command',
            JointTrajectory,
            queue_size=2)
        #self.head_goal_pub = rospy.Publisher('/head_controller/point_head/goal', PointHeadActionGoal, queue_size=2)
        self.gripper_client = actionlib.SimpleActionClient(
            'gripper_controller/gripper_action', GripperCommandAction)

        self.arm_cartesian_twist_pub = rospy.Publisher(
            '/arm_controller/cartesian_twist/command', Twist, queue_size=2)

        self.head_point_client = actionlib.SimpleActionClient(
            'head_controller/point_head', PointHeadAction)

        self.arm_move_group = MoveGroupInterface("arm",
                                                 "base_link",
                                                 plan_only=True)
        self.arm_trajectory_client = actionlib.SimpleActionClient(
            "arm_controller/follow_joint_trajectory",
            FollowJointTrajectoryAction)
        self.arm_trajectory_client.wait_for_server()

        if 0:
            logger.info('Creating MoveGroupInterface...')
            self.move_group = MoveGroupInterface('arm_with_torso',
                                                 'base_link',
                                                 plan_only=True)
            logger.info('Created MoveGroupInterface')
            if 0:
                logger.info('Creating PlanningSceneInterface...')
                self.planning_scene = PlanningSceneInterface('base_link')
                self.planning_scene.removeCollisionObject('my_front_ground')
                self.planning_scene.removeCollisionObject('my_back_ground')
                self.planning_scene.removeCollisionObject('my_right_ground')
                self.planning_scene.removeCollisionObject('my_left_ground')
                self.planning_scene.addCube('my_front_ground', 2, 1.1, 0.0,
                                            -1.0)
                self.planning_scene.addCube('my_back_ground', 2, -1.2, 0.0,
                                            -1.0)
                self.planning_scene.addCube('my_left_ground', 2, 0.0, 1.2,
                                            -1.0)
                self.planning_scene.addCube('my_right_ground', 2, 0.0, -1.2,
                                            -1.0)

        logger.warn('FetchRobotGymEnv ROS node running')

        self.head_point_client.wait_for_server()
        logger.warn('FetchRobotGymEnv ROS node connected')
Ejemplo n.º 32
0
 def __init__(self):
     self.scene = PlanningSceneInterface("base_link")
     self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
     self.move_group = MoveGroupInterface("arm", "base_link")
Ejemplo n.º 33
0
def main():
    rospy.init_node('a5_obstacles')
    wait_for_time()

    planning_scene = PlanningSceneInterface('base_link')
    planning_scene.clear()
    planning_scene.removeCollisionObject('table')
    planning_scene.removeCollisionObject('floor')
    planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01 / 2)
    planning_scene.addBox('table', 0.5, 1, 0.82, 0.8, 0, 0.72 / 2)
    planning_scene.addBox('robot_base', 0.54, 0.52, 0.37, 0, 0, 0.37 / 2)

    rospy.sleep(2)
    rospy.spin()
Ejemplo n.º 34
0
    def __init__(self):
        self.moving_mode = False
        self.plan_only = False
        self.prev_joy_buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]

        self.joint_names = [
            "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
            "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
            "wrist_roll_joint"
        ]

        self.joy_subscriber = rospy.Subscriber("/fetch_joy", Joy,
                                               self.joy_callback)
        self.joints_subscriber = rospy.Subscriber("/joint_states", JointState,
                                                  self.joints_states_callback)
        self.rgbimage_subscriber = rospy.Subscriber("/head_camera/depth/image",
                                                    Image,
                                                    self.depthimage_callback)
        self.depthimage_subscriber = rospy.Subscriber(
            "/head_camera/depth/image", Image, self.rgbimage_callback)

        self.reset_subscriber = rospy.Subscriber("/fetch_ai_arm_reset_pos",
                                                 Empty,
                                                 self.arm_reset_callback)
        self.reset_subscriber = rospy.Subscriber("/fetch_ai_arm_start_pos",
                                                 Empty,
                                                 self.arm_start_callback)

        self.arm_effort_pub = rospy.Publisher(
            "/arm_controller/weightless_torque/command",
            JointTrajectory,
            queue_size=2)
        self.gripper_client = actionlib.SimpleActionClient(
            "gripper_controller/gripper_action", GripperCommandAction)
        self.arm_cartesian_twist_pub = rospy.Publisher(
            "/arm_controller/cartesian_twist/command", Twist, queue_size=2)
        self.head_point_client = actionlib.SimpleActionClient(
            "head_controller/point_head", PointHeadAction)
        self.arm_move_group = MoveGroupInterface("arm",
                                                 "base_link",
                                                 plan_only=self.plan_only)

        planning_scene = PlanningSceneInterface("base_link")
        planning_scene.removeCollisionObject("my_front_ground")
        planning_scene.removeCollisionObject("my_back_ground")
        planning_scene.removeCollisionObject("my_right_ground")
        planning_scene.removeCollisionObject("my_left_ground")
        planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
        planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
        planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
        planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
Ejemplo n.º 35
0
# Note: roslaunch fetch_moveit_config move_group.launch must be running
# Safety!: Do NOT run this script near people or objects.
# Safety!: There is NO perception.
#          The ONLY objects the collision detection software is aware
#          of are itself & the floor.
if __name__ == '__main__':
    rospy.init_node("fit_moveit")

    # Create move group interface for a fetch robot
    move_group = MoveGroupInterface("arm_with_torso", "base_link")

    # Define ground plane
    # This creates objects in the planning scene that mimic the ground
    # If these were not in place gripper could hit the ground
    planning_scene = PlanningSceneInterface("base_link")
    # planning_scene.removeCollisionObject("my_front_ground")
    # planning_scene.removeCollisionObject("my_back_ground")
    # planning_scene.removeCollisionObject("my_right_ground")
    # planning_scene.removeCollisionObject("my_left_ground")
    # planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
    # planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
    # planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
    # planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)

    # This is the wrist link not the gripper itself
    gripper_frame = 'wrist_roll_link'
    # Position and rotation of two "wave end poses"
    pose = Pose(Point(0.17, 1.545, 1.822),
                Quaternion(-0.374, -0.701, 0.173, 0.635))
Ejemplo n.º 36
0
class GraspingClient(object):

    def __init__(self):
        self.scene = PlanningSceneInterface("base_link")
        self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
        self.move_group = MoveGroupInterface("arm", "base_link")

        find_topic = "basic_grasping_perception/find_objects"
        rospy.loginfo("Waiting for %s..." % find_topic)
        self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
        self.find_client.wait_for_server()

    def updateScene(self):
        # find objects
        goal = FindGraspableObjectsGoal()
        goal.plan_grasps = True
        self.find_client.send_goal(goal)
        self.find_client.wait_for_result(rospy.Duration(5.0))
        find_result = self.find_client.get_result()

        rospy.loginfo("Found %d objects" % len(find_result.objects))

        # remove previous objects
        #for name in self.scene.getKnownCollisionObjects():
        #    self.scene.removeCollisionObject(name, False)
        #for name in self.scene.getKnownAttachedObjects():
        #    self.scene.removeAttachedObject(name, False)
        self.scene.waitForSync()

        # insert objects to scene
        objects = list()
        idx = -1
        for obj in find_result.objects:
            idx += 1
            obj.object.name = "obj%d"%idx
            self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], use_service=False)
            if obj.object.primitive_poses[0].position.x < 1.3: #< 0.85:
                objects.append([obj, obj.object.primitive_poses[0].position.y])

        #for obj in find_result.support_surfaces:
        #    # extend surface to floor, and make wider since we have narrow field of view
        #    height = obj.primitive_poses[0].position.z
        #    obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], 1.5,  # wider
        #                                    obj.primitives[0].dimensions[2] + height]
        #    obj.primitive_poses[0].position.z += -height/2.0

        #    # add to scene
        #    self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0])
        self.scene.waitForSync()

        # store for grasping
        self.surfaces = find_result.support_surfaces

        # store graspable objects by y
        objects.sort(key=lambda object: object[1])
        #objects.reverse()
        self.objects = [object[0] for object in objects]

    def getGraspableObject(self, goal_obj_x, goal_obj_y):
        graspable = None
        for obj in self.objects:
            # need grasps
            if len(obj.grasps) < 1:
                continue
            # check size
            if obj.object.primitives[0].dimensions[0] < 0.03 or \
               obj.object.primitives[0].dimensions[0] > 0.25 or \
               obj.object.primitives[0].dimensions[0] < 0.03 or \
               obj.object.primitives[0].dimensions[0] > 0.25 or \
               obj.object.primitives[0].dimensions[0] < 0.03 or \
               obj.object.primitives[0].dimensions[0] > 0.25:
                continue
            # has to be on table
            if obj.object.primitive_poses[0].position.z < 0.4:
                continue
            if obj.object.primitive_poses[0].position.x < 0.3:
                continue
            # get goal object
            if ( abs(obj.object.primitive_poses[0].position.x - goal_obj_x) > 0.03):
                continue
            if ( abs(obj.object.primitive_poses[0].position.y - goal_obj_y) > 0.03):
                continue
            print("**************************************Object Name: ", obj.object.name)
            print("Object Pose: ", obj.object.primitive_poses[0], obj.object.primitives[0])

            return obj.object, obj.grasps
        # nothing detected
        return None, None

    def getSupportSurface(self, name):
        for surface in self.support_surfaces:
            if surface.name == name:
                return surface
        return None

    def getPlaceLocation(self):
        pass

    def pick(self, block, grasps):
        success, pick_result = self.pickplace.pick_with_retry(block.name, grasps, support_name=block.support_surface, scene=self.scene)
        self.pick_result = pick_result
        #hold the object
        #touch_links = ['l_gripper_finger_link', 'r_gripper_finger_link']
        #self.hand_group.attach_object('apple', 'base_link', touch_links=touch_links)
        return success

    def place(self, block, pose_stamped):
        places = list()
        l = PlaceLocation()
        l.place_pose.pose = pose_stamped.pose
        l.place_pose.header.frame_id = pose_stamped.header.frame_id

        # copy the posture, approach and retreat from the grasp used
        l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
        l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
        l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
        places.append(copy.deepcopy(l))
        # create another several places, rotate each by 360/m degrees in yaw direction
        m = 16 # number of possible place poses
        pi = 3.141592653589
        for i in range(0, m-1):
            #l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m)
            places.append(copy.deepcopy(l))

        success, place_result = self.pickplace.place_with_retry(block.name, places, scene=self.scene)
        return success

    def tuck(self):
        joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
                  "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
        pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
        while not rospy.is_shutdown():
            result = self.move_group.moveToJointPosition(joints, pose, 0.02)
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                return

    def stow(self):
        joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
                  "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
        pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
        #pose = [-1.60, -1.10, -1.20, -1.50, 0.0, -1.51, 0.0]
        while not rospy.is_shutdown():
            result = self.move_group.moveToJointPosition(joints, pose, 0.02)
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                return

    def intermediate_stow(self):
        joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
                  "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
        pose = [0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0]
        while not rospy.is_shutdown():
            result = self.move_group.moveToJointPosition(joints, pose, 0.02)
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                return
Ejemplo n.º 37
0
class Grasping(object):
    def __init__(self):
        self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
        self.scene = PlanningSceneInterface("base_link")
        self.move_group = MoveGroupInterface("arm", "base_link")

        find_objects = "basic_grasping_perception/find_objects"

        self.find_client = actionlib.SimpleActionClient(
            find_objects, FindGraspableObjectsAction)
        self.find_client.wait_for_server()

    def pickup(self, block, grasps):
        rospy.sleep(1)

        success, pick_result = self.pickplace.pick_with_retry(
            block.name,
            grasps,
            support_name=block.support_surface,
            scene=self.scene)

        self.pick_result = pick_result
        return success

    #swaps two blocks positions.
    def swapBlockPos(self, block1Pos, block2Pos):
        #intermediate point for movement

        self.armIntermediatePose()

        #intermediate positon on the table, used for short term storage of the first block manipulated in the sorting.
        posIntermediate = np.array([0.725, 0])

        self.armIntermediatePose()

        # Get block to pick
        while not rospy.is_shutdown():
            rospy.loginfo("Picking object...")
            self.updateScene()
            cube, grasps = self.getGraspableObject(block1Pos)
            if cube == None:
                rospy.logwarn("Perception failed.")
                continue

            # Pick the block
            if self.pickup(cube, grasps):
                break
            rospy.logwarn("Grasping failed.")

        #self.tuck()
        #Place the block
        while not rospy.is_shutdown():
            rospy.loginfo("Placing object...")
            pose = PoseStamped()
            pose.pose = cube.primitive_poses[0]
            pose.pose.position.z += 0.05
            pose.header.frame_id = cube.header.frame_id
            if self.place(cube, pose, posIntermediate):
                break
            rospy.logwarn("Placing failed.")

        #place block 2 in block 1's
        self.armIntermediatePose()
        # Get block to pick
        while not rospy.is_shutdown():
            rospy.loginfo("Picking object...")
            self.updateScene()
            cube, grasps = self.getGraspableObject(block2Pos)
            if cube == None:
                rospy.logwarn("Perception failed.")
                continue

            # Pick the block
            if self.pickup(cube, grasps):
                break
            rospy.logwarn("Grasping failed.")

        while not rospy.is_shutdown():
            rospy.loginfo("Placing object...")
            pose = PoseStamped()
            pose.pose = cube.primitive_poses[0]
            pose.pose.position.z += 0.05
            pose.header.frame_id = cube.header.frame_id
            if self.place(cube, pose, block1Pos):
                break
            rospy.logwarn("Placing failed.")

        #place block1 in block 2's spot
        self.armIntermediatePose()

        while not rospy.is_shutdown():
            rospy.loginfo("Picking object...")
            self.updateScene()
            cube, grasps = self.getGraspableObject(posIntermediate)
            if cube == None:
                rospy.logwarn("Perception failed.")
                continue

            # Pick the block
            if self.pickup(cube, grasps):
                break
            rospy.logwarn("Grasping failed.")

        #self.tuck()
        self.armIntermediatePose()

        while not rospy.is_shutdown():
            rospy.loginfo("Placing object...")
            pose = PoseStamped()
            pose.pose = cube.primitive_poses[0]
            pose.pose.position.z += 0.05
            pose.header.frame_id = cube.header.frame_id
            if self.place(cube, pose, block2Pos):
                break
            rospy.logwarn("Placing failed.")

        return

    def place(self, block, pose_stamped, placePos):

        rospy.sleep(1)
        #creates a list of place positons for the block, with a specified x, y, and z.

        places = list()
        l = PlaceLocation()
        l.place_pose.pose = pose_stamped.pose
        l.place_pose.header.frame_id = "base_link"

        l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
        l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
        l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat

        #this x and y value are input as placePos through the function call.
        l.place_pose.pose.position.x = placePos[0]
        l.place_pose.pose.position.y = placePos[1]

        places.append(copy.deepcopy(l))

        #success = self.pickplace.place_with_retry(block.name, places, scene = self.scene)

        #return success

        ## create another several places, rotate each by 360/m degrees in yaw direction
        m = 16  # number of possible place poses
        pi = 3.141592653589
        for i in range(0, m - 1):
            l.place_pose.pose = rotate_pose_msg_by_euler_angles(
                l.place_pose.pose, 0, 0, 2 * pi / m)
            places.append(copy.deepcopy(l))

        success, place_result = self.pickplace.place_with_retry(
            block.name, places, scene=self.scene)
        return success

    def updateScene(self):
        rospy.sleep(1)
        # find objectsw
        goal = FindGraspableObjectsGoal()
        goal.plan_grasps = True
        self.find_client.send_goal(goal)
        self.find_client.wait_for_result(rospy.Duration(5.0))
        find_result = self.find_client.get_result()

        # remove previous objects
        for name in self.scene.getKnownCollisionObjects():
            self.scene.removeCollisionObject(name, False)
        for name in self.scene.getKnownAttachedObjects():
            self.scene.removeAttachedObject(name, False)
        self.scene.waitForSync()

        # insert objects to scene
        idx = -1
        for obj in find_result.objects:
            idx += 1
            obj.object.name = "object%d" % idx
            self.scene.addSolidPrimitive(obj.object.name,
                                         obj.object.primitives[0],
                                         obj.object.primitive_poses[0],
                                         use_service=False)

        for obj in find_result.support_surfaces:
            # extend surface to floor, and make wider since we have narrow field of view
            height = obj.primitive_poses[0].position.z
            obj.primitives[0].dimensions = [
                obj.primitives[0].dimensions[0],
                1.5,  # wider
                obj.primitives[0].dimensions[2] + height
            ]
            obj.primitive_poses[0].position.z += -height / 2.0

            # add to scene
            self.scene.addSolidPrimitive(obj.name,
                                         obj.primitives[0],
                                         obj.primitive_poses[0],
                                         use_service=True)

        self.scene.waitForSync()

        # store for grasping
        self.objects = find_result.objects
        self.surfaces = find_result.support_surfaces

    def getSupportSurface(self, name):
        for surface in self.support_surfaces:
            if surface.name == name:
                return surface
        return None

    def getPlaceLocation(self):
        pass

    def getGraspableObject(self, pos):
        graspable = None

        for obj in self.objects:
            if len(obj.grasps) < 1:
                continue
            if (obj.object.primitives[0].dimensions[0] < 0.05 or \
                obj.object.primitives[0].dimensions[0] > 0.07 or \
                obj.object.primitives[0].dimensions[1] < 0.05 or \
                obj.object.primitives[0].dimensions[1] > 0.07 or \
                obj.object.primitives[0].dimensions[2] < 0.05 or \
                obj.object.primitives[0].dimensions[2] > 0.07):

                continue

            if (obj.object.primitive_poses[0].position.z < 0.3) or \
                (obj.object.primitive_poses[0].position.y > pos[1]+0.05) or \
                (obj.object.primitive_poses[0].position.y < pos[1]-0.05) or \
                (obj.object.primitive_poses[0].position.x > pos[0]+0.05) or \
                (obj.object.primitive_poses[0].position.x < pos[0]-0.05):
                continue
            return obj.object, obj.grasps

        return None, None

    def armToXYZ(self, x, y, z):
        rospy.sleep(1)
        #new pose_stamped of the end effector that moves the arm out of the way of the vision for planning.
        intermediatePose = PoseStamped()

        intermediatePose.header.frame_id = 'base_link'

        #position
        intermediatePose.pose.position.x = x
        intermediatePose.pose.position.y = y
        intermediatePose.pose.position.z = z

        #quaternion for the end position

        intermediatePose.pose.orientation.w = 1

        while not rospy.is_shutdown():
            result = self.move_group.moveToPose(intermediatePose,
                                                "wrist_roll_link")
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                return

    def armIntermediatePose(self):

        self.armToXYZ(0.1, -0.7, 0.9)

    def tuck(self):
        joints = [
            "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
            "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
            "wrist_roll_joint"
        ]
        pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
        while not rospy.is_shutdown():
            result = self.move_group.moveToJointPosition(joints, pose, 0.02)
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                return
Ejemplo n.º 38
0
    parser = argparse.ArgumentParser(
        description="Remove objects from the MoveIt planning scene.")
    parser.add_argument("name",
                        nargs="?",
                        help="Name of object to remove")
    parser.add_argument("--all",
                        help="Remove all objects.",
                        action="store_true")
    parser.add_argument("--attached",
                        help="Remove an attached object.",
                        action="store_true")
    args = parser.parse_args()

    if args.all:
        rospy.init_node("remove_objects")
        scene = PlanningSceneInterface("base_link")
        for name in scene.getKnownCollisionObjects():
            print("Removing %s" % name)
            scene.removeCollisionObject(name, use_service=False)
        scene.waitForSync()
    elif args.name:
        rospy.init_node("remove_objects")
        scene = PlanningSceneInterface("base_link")
        print("Removing %s" % args.name)
        if args.attached:
            scene.removeAttachedObject(args.name)
        else:
            scene.removeCollisionObject(args.name)
    else:
        parser.print_help()
Ejemplo n.º 39
0
 def __init__(self):
     self._scene = PlanningSceneInterface('base_link')
     self._pickplace = PickPlaceInterface('xarm5', 'gripper', verbose=True)
     self._move_group = MoveGroupInterface('xarm5', 'base_link')
Ejemplo n.º 40
0
class GraspingClient(object):
    def __init__(self, sim=False):
        # Define planning groups
        self.scene = PlanningSceneInterface("base_link")
        self.dof = rospy.get_param('~jaco_dof')
        self.move_group = MoveGroupInterface("upper_body", "base_link")
        self.rmove_group = MoveGroupInterface("right_arm", "base_link")

        planner_id = "RRTConnectkConfigDefault"
        self.move_group.setPlannerId(planner_id)
        self.rmove_group.setPlannerId(planner_id)

        self.objects_heights = [0.122, 0.240]
        self.objects_heights_tolerances = [0.02, 0.03]

        # Define joints and positions for 6 and 7 DOF
        if "6dof" == self.dof:
            self._upper_body_joints = [
                "right_elbow_joint", "right_shoulder_lift_joint",
                "right_shoulder_pan_joint", "right_wrist_1_joint",
                "right_wrist_2_joint", "right_wrist_3_joint",
                "left_elbow_joint", "left_shoulder_lift_joint",
                "left_shoulder_pan_joint", "left_wrist_1_joint",
                "left_wrist_2_joint", "left_wrist_3_joint", "linear_joint",
                "pan_joint", "tilt_joint"
            ]
            self._right_arm_joints = [
                "right_elbow_joint", "right_shoulder_lift_joint",
                "right_shoulder_pan_joint", "right_wrist_1_joint",
                "right_wrist_2_joint", "right_wrist_3_joint"
            ]
            self._left_arm_joints = [
                "left_elbow_joint", "left_shoulder_lift_joint",
                "left_shoulder_pan_joint", "left_wrist_1_joint",
                "left_wrist_2_joint", "left_wrist_3_joint"
            ]
            self.tucked = [
                -2.8, -1.48, -1.48, 0, 0, 1.571, 2.8, 1.48, 1.48, 0, 0, -1.571,
                0.0371, 0.0, 0.0
            ]
            self.constrained_stow = [
                2.28, 2.00, -2.56, -0.09, 0.15, 1.082, -2.28, -2.17, 2.56,
                0.09, -0.15, 2.06, 0.42, 0.0, 0.0
            ]
            self.rarm_const_stow = [2.28, 2.00, -2.56, -0.09, 0.15, 1.06]
            #self.rarm_const_stow = [-1.51, -0.22, -2.00, -2.04, 1.42, 1.32]

        elif "7dof" == self.dof:
            self._upper_body_joints = [
                "right_shoulder_pan_joint", "right_shoulder_lift_joint",
                "right_arm_half_joint", "right_elbow_joint",
                "right_wrist_spherical_1_joint",
                "right_wrist_spherical_2_joint", "right_wrist_3_joint",
                "left_shoulder_pan_joint", "left_shoulder_lift_joint",
                "left_arm_half_joint", "left_elbow_joint",
                "left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
                "left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint"
            ]
            self._right_arm_joints = [
                "right_shoulder_pan_joint", "right_shoulder_lift_joint",
                "right_arm_half_joint", "right_elbow_joint",
                "right_wrist_spherical_1_joint",
                "right_wrist_spherical_2_joint", "right_wrist_3_joint"
            ]
            self._left_arm_joints = [
                "left_shoulder_pan_joint", "left_shoulder_lift_joint",
                "left_arm_half_joint", "left_elbow_joint",
                "left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
                "left_wrist_3_joint"
            ]
            self.tucked = [
                -1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7,
                0.0, -0.5, 1.7, 0.04, 0, 0
            ]
            self.constrained_stow = [
                -2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0,
                0.0, -1.0, 0.42, 0, 0
            ]
            self.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0]

        else:
            rospy.logerr("DoF needs to be set 6 or 7, aborting demo")
            return

        # Define the MoveIt pickplace pipeline
        self.pickplace = None
        self.pickplace = PickPlaceInterface("left_side",
                                            "left_gripper",
                                            verbose=True)
        self.pickplace.planner_id = planner_id
        self.pick_result = None
        self.place_result = None

        self._listener = tf.TransformListener()

        self._lgripper = GripperActionClient('left')

        find_grasp_planning_topic = "grasp_planner/plan"
        rospy.loginfo("Waiting for %s..." % find_grasp_planning_topic)
        self.find_grasp_planning_client = actionlib.SimpleActionClient(
            find_grasp_planning_topic, GraspPlanningAction)
        self.find_grasp_planning_client.wait_for_server()
        rospy.loginfo("...connected")

        self.scene.clear()

        # This is a simulation so need to adjust gripper parameters
        if sim:
            self._gripper_closed = 0.96
            self._gripper_open = 0.00
        else:
            self._gripper_closed = 0.01
            self._gripper_open = 0.165

    def __del__(self):
        self.scene.clear()

    def add_objects_to_keep(self, obj):
        self._objs_to_keep.append(obj)

    def clearScene(self):
        self.scene.clear()

    def calculateGraspForObject(self, object_to_grasp, gripper):
        goal = GraspPlanningGoal()
        goal.object = object_to_grasp
        goal.gripper = gripper
        self.find_grasp_planning_client.send_goal(goal)
        self.find_grasp_planning_client.wait_for_result(rospy.Duration(5.0))

        return self.find_grasp_planning_client.get_result(
        ).grasps  #moveit_msgs/Grasp[]

    def pick(self, block, grasps):
        success, pick_result = self.pickplace.pick_with_retry(block.name,
                                                              grasps,
                                                              retries=1,
                                                              scene=self.scene)
        self.pick_result = pick_result
        return success

    def place(self, block, pose_stamped):
        places = list()
        l = PlaceLocation()
        l.place_pose.pose = pose_stamped.pose
        l.place_pose.header.frame_id = pose_stamped.header.frame_id

        # copy the posture, approach and retreat from the grasp used
        l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
        l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
        l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
        places.append(copy.deepcopy(l))
        # create another several places, rotate each by 360/m degrees in yaw direction
        m = 16  # number of possible place poses
        pi = 3.141592653589
        for i in range(0, m - 1):
            l.place_pose.pose = rotate_pose_msg_by_euler_angles(
                l.place_pose.pose, 0, 0, 2 * pi / m)
            places.append(copy.deepcopy(l))

        success, place_result = self.pickplace.place_with_retry(
            block.name, places, retries=1, scene=self.scene)
        self.place_result = place_result
        return success

    def goto_tuck(self):
        # remove previous objects
        while not rospy.is_shutdown():
            result = self.move_group.moveToJointPosition(
                self._upper_body_joints, self.tucked, 0.05)
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                return

    def goto_plan_grasp(self):
        while not rospy.is_shutdown():
            result = self.move_group.moveToJointPosition(
                self._upper_body_joints, self.constrained_stow, 0.05)
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                return

    def left_arm_constrained_stow(self):
        c1 = Constraints()
        c1.orientation_constraints.append(OrientationConstraint())
        c1.orientation_constraints[0].header.stamp = rospy.get_rostime()
        c1.orientation_constraints[0].header.frame_id = "base_link"
        c1.orientation_constraints[0].link_name = "left_ee_link"
        c1.orientation_constraints[0].orientation.w = 1.0
        c1.orientation_constraints[
            0].absolute_x_axis_tolerance = 0.2  #x axis is pointed up for wrist link
        c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2
        c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28
        c1.orientation_constraints[0].weight = 1.0

        while not rospy.is_shutdown():
            result = self.lmove_group.moveToJointPosition(
                self._left_arm_joints,
                self.larm_const_stow,
                0.05,
                path_constraints=c1,
                planning_time=120.0)
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                return

    def right_arm_constrained_stow(self):
        # c1 = Constraints()
        # c1.orientation_constraints.append(OrientationConstraint())
        # c1.orientation_constraints[0].header.stamp = rospy.get_rostime()
        # c1.orientation_constraints[0].header.frame_id = "base_link"
        # c1.orientation_constraints[0].link_name = "right_ee_link"
        # c1.orientation_constraints[0].orientation.w=1.0
        # c1.orientation_constraints[0].absolute_x_axis_tolerance = 0.2 #x axis is pointed up for wrist link
        # c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2
        # c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28
        # c1.orientation_constraints[0].weight = 1.0

        while not rospy.is_shutdown():
            result = self.rmove_group.moveToJointPosition(
                self._right_arm_joints,
                self.rarm_const_stow,
                0.05,
                planning_time=120.0)
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                return

    def open_gripper(self):
        self._lgripper.command(self._gripper_open, block=True)

    def close_gripper(self):
        self._lgripper.command(self._gripper_closed, block=True)
Ejemplo n.º 41
0
class Reaching:
    def __init__(self):
        rospy.init_node('reaching', anonymous=True)
        self.robot = moveit_commander.RobotCommander()
        self.model_pose = [0.0,0.0,0.0]
        self.set_forbidden()
        self.set_init_pose()

    def set_forbidden(self):
        #set forbidden erea
        self.planning_scene = PlanningSceneInterface("base_link")
        self.planning_scene.removeCollisionObject("my_front_ground")
        self.planning_scene.removeCollisionObject("my_back_ground")
        self.planning_scene.removeCollisionObject("my_right_ground")
        self.planning_scene.removeCollisionObject("my_left_ground")
        self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
        self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
        self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
        self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)

    def set_init_pose(sefl): 
        #set init pose
        move_group = MoveGroupInterface("manipulator","base_link")
        planning_scene = PlanningSceneInterface("base_link")
        joint_names = ["shoulder_pan_joint", "shoulder_lift_joint",
                        "elbow_joint", "wrist_1_joint",
                        "wrist_2_joint", "wrist_3_joint"]
        pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0]      
        move_group.moveToJointPosition(joint_names, pose, wait=False)
        move_group.get_move_action().wait_for_result()
        result = move_group.get_move_action().get_result()
        move_group.get_move_action().cancel_all_goals()

    def callback(self,data):
        #print data.markers[0].pose.position
        x = data.markers[0].pose.position.x
        y = data.markers[0].pose.position.y
        z = data.markers[0].pose.position.z
        self.model_pose = [x,y,z]

    def start_subscriber(self):
        rospy.Subscriber("/visualization_marker_array", MarkerArray, self.callback)

    def publish_tf(self):
        br = tf.TransformBroadcaster()
        br.sendTransform((self.model_pose[0], self.model_pose[1], self.model_pose[2]), (0.0, 0.0, 0.0, 1.0), rospy.Time.now(), "object", "depth_camera_frame")


    def target(self):
        rate = rospy.Rate(1)
        rospy.sleep(1)
        ls = tf.TransformListener()
        while not rospy.is_shutdown():
            self.publish_tf()

            try:
                (trans,rot) = ls.lookupTransform('/base_link', '/object', rospy.Time(0))
                print "tf base_link to test",trans

                group = moveit_commander.MoveGroupCommander("manipulator")
                #print group.get_current_pose().pose
                pose_target = geometry_msgs.msg.Pose()
                pose = group.get_current_pose().pose
                pose_target = geometry_msgs.msg.Pose()
                pose_target.orientation.x = pose.orientation.x
                pose_target.orientation.y = pose.orientation.y
                pose_target.orientation.z = pose.orientation.z
                pose_target.orientation.w = pose.orientation.w
                pose_target.position.x = trans[0]
                pose_target.position.y = trans[1]
                pose_target.position.z = trans[2]+0.2

                group.set_pose_target(pose_target)
                group.go()
                
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                print "e"

            rospy.sleep(1)
            self.set_init_pose()
Ejemplo n.º 42
0
class GraspingClient(object):

    def __init__(self):
        self.scene = PlanningSceneInterface("base_link")
        self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
        self.move_group = MoveGroupInterface("arm", "base_link")

        find_topic = "basic_grasping_perception/find_objects"
        rospy.loginfo("Waiting for %s..." % find_topic)
        self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
        self.find_client.wait_for_server()

    def updateScene(self):
        # find objects
	rospy.loginfo("get new grasps")
        goal = FindGraspableObjectsGoal()
        goal.plan_grasps = True
        self.find_client.send_goal(goal)
        self.find_client.wait_for_result(rospy.Duration(5.0))
        find_result = self.find_client.get_result()

        # remove previous objects
        for name in self.scene.getKnownCollisionObjects():
            self.scene.removeCollisionObject(name, False)
        for name in self.scene.getKnownAttachedObjects():
            self.scene.removeAttachedObject(name, False)

        self.scene.removeCollisionObject("my_front_ground")
        self.scene.removeCollisionObject("my_back_ground")
        self.scene.removeCollisionObject("my_right_ground")
        self.scene.removeCollisionObject("my_left_ground")
        self.scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
        self.scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
        self.scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
        self.scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
	# TODO: ADD BOX
        self.scene.waitForSync()

        # insert objects to scene
        idx = -1
        for obj in find_result.objects:
            idx += 1
            obj.object.name = "object%d"%idx
            self.scene.addSolidPrimitive(obj.object.name,
                                         obj.object.primitives[0],
                                         obj.object.primitive_poses[0],
                                         wait = False)

        for obj in find_result.support_surfaces:
            # extend surface to floor, and make wider since we have narrow field of view
            height = obj.primitive_poses[0].position.z
	    # added + .1
            obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
                                            1.5,  # wider
                                            obj.primitives[0].dimensions[2] + height]
            obj.primitive_poses[0].position.z += -height/2.0

            # add to scene
            self.scene.addSolidPrimitive(obj.name,
                                         obj.primitives[0],
                                         obj.primitive_poses[0],
                                         wait = False)
        self.scene.waitForSync()

        # store for grasping
        self.objects = find_result.objects
        self.surfaces = find_result.support_surfaces

    def getSupportSurface(self, name):
        for surface in self.support_surfaces:
            if surface.name == name:
                return surface
        return None

    def tuck(self):
        joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
                  "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
        pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
        while not rospy.is_shutdown():
            result = self.move_group.moveToJointPosition(joints, pose, 0.02)
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                return

    def zero(self):
        joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
                  "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
        pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        while not rospy.is_shutdown():
            result = self.move_group.moveToJointPosition(joints, pose, 0.02)
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                return
Ejemplo n.º 43
0
def picknplace():
    # Define initial parameters
    p = PlanningSceneInterface("base")
    g = MoveGroupInterface("both_arms", "base")
    gr = MoveGroupInterface("right_arm", "base")
    gl = MoveGroupInterface("left_arm", "base")
    leftgripper = baxter_interface.Gripper('left')
    leftgripper.calibrate()
    leftgripper.open()
    jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
    jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
    jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2']
    pos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
    pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
    lpos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519]
    lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384]    
    rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
    rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
    
    # Clear planning scene
    p.clear()
    # Add table as attached object
    p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])

    # Move both arms to start state              
    g.moveToJointPosition(jts_both, pos1, plan_only=False)
    
    # Get obj locations
    temp = rospy.wait_for_message("Dpos", PoseArray)
    locs = temp.poses 
    locs_x = []
    locs_y = []
    orien = []
    size = []

    for i in range(len(locs)):
        locs_x.append(0.57 + locs[i].position.x)
        locs_y.append(-0.011 + locs[i].position.y)
        orien.append(locs[i].position.z*pi/180)
        size.append(locs[i].orientation.x)

    # Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
    ind_rmv = []
    for i in range(0,len(locs)):
        if (locs_y[i] > 0.42):
            ind_rmv.append(i)
            continue
        for j in range(i,len(locs)):
            if not (i == j):
                if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
                    ind_rmv.append(i)
    
    locs_x = del_meth(locs_x, ind_rmv)
    locs_y = del_meth(locs_y, ind_rmv)
    orien = del_meth(orien, ind_rmv) 
    size = del_meth(size, ind_rmv)

    # Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
    if locs_x:  
        ig0 = itemgetter(0)
        sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))

        locs_x = list(sorted_lists[1])
        locs_y = list(sorted_lists[2])
        orien = list(sorted_lists[3])
        size = list(sorted_lists[0])

    # Loop to continue pick and place until all objects are cleared from table
    j=0
    k=0
    while locs_x:
        p.clear() # CLear planning scene of all collision objects

        # Attach table as attached collision object to base of baxter
        p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])
        xn = locs_x[0]
        yn = locs_y[0]
        zn = -0.06
        thn = orien[0]
        sz = size[0]
        if thn > pi/4:
            thn = -1*(thn%(pi/4))

        # Add collision objects into planning scene
        objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']
        for i in range(1,len(locs_x)):
            p.addCube(objlist[i], 0.05, locs_x[i], locs_y[i], -0.05)
        p.waitForSync()        

        # Move both arms to pos2 (Right arm away and left arm on table)
        g.moveToJointPosition(jts_both, pos2, plan_only=False)

        # Move left arm to pick object and pick object
        pickgoal = PoseStamped() 
        pickgoal.header.frame_id = "base"
        pickgoal.header.stamp = rospy.Time.now()
        pickgoal.pose.position.x = xn
        pickgoal.pose.position.y = yn
        pickgoal.pose.position.z = zn+0.1
        pickgoal.pose.orientation.x = 1.0
        pickgoal.pose.orientation.y = 0.0
        pickgoal.pose.orientation.z = 0.0
        pickgoal.pose.orientation.w = 0.0
        gl.moveToPose(pickgoal, "left_gripper", plan_only=False)

        # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles 
        pickgoal.pose = rotate_pose_msg_by_euler_angles(pickgoal.pose, 0.0, 0.0, thn)
        gl.moveToPose(pickgoal, "left_gripper", plan_only=False) 

        pickgoal.pose.position.z = zn
        gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 0.14, plan_only=False)
        leftgripper.close()

        pickgoal.pose.position.z = zn+0.1
        gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
        
        # Move both arms to pos1
        g.moveToJointPosition(jts_both, pos1, plan_only=False)

        # Get obj locations
        temp = rospy.wait_for_message("Dpos", PoseArray)
        locs = temp.poses 
        locs_x = []
        locs_y = []
        orien = []
        size = []

        for i in range(len(locs)):
            locs_x.append(0.57 + locs[i].position.x)
            locs_y.append(-0.011 + locs[i].position.y)
            orien.append(locs[i].position.z*pi/180)
            size.append(locs[i].orientation.x)

        # Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
        ind_rmv = []
        for i in range(0,len(locs)):
            if (locs_y[i] > 0.42):
                ind_rmv.append(i)
                continue
            for j in range(i,len(locs)):
                if not (i == j):
                    if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
                        ind_rmv.append(i)
        
        locs_x = del_meth(locs_x, ind_rmv)
        locs_y = del_meth(locs_y, ind_rmv)
        orien = del_meth(orien, ind_rmv) 
        size = del_meth(size, ind_rmv)

        # Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
        if locs_x:  
            ig0 = itemgetter(0)
            sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))

            locs_x = list(sorted_lists[1])
            locs_y = list(sorted_lists[2])
            orien = list(sorted_lists[3])
            size = list(sorted_lists[0])

        # Place object
        stleft = PoseStamped() 
        stleft.header.frame_id = "base"
        stleft.header.stamp = rospy.Time.now()
        stleft.pose.position.x = 0.65
        stleft.pose.position.y = 0.55
        stleft.pose.position.z = 0.1
        stleft.pose.orientation.x = 1.0
        stleft.pose.orientation.y = 0.0
        stleft.pose.orientation.z = 0.0
        stleft.pose.orientation.w = 0.0

        # Stack objects (large cubes) if size if greater than some threshold
        if sz > 16.:            
            stleft.pose.position.x = 0.65
            stleft.pose.position.y = 0.7
            stleft.pose.position.z = -0.04+(k*0.05)
            k += 1
        
        gl.moveToPose(stleft, "left_gripper", plan_only=False)        
        leftgripper.open()

        stleft.pose.position.z = 0.3
        gl.moveToPose(stleft, "left_gripper", plan_only=False)
Ejemplo n.º 44
0
class Reaching:
    def __init__(self):
        rospy.init_node('reaching', anonymous=True)
        self.robot = moveit_commander.RobotCommander()
        # self.model_pose = [0.0,0.2,0.2  , 0,0,0,1]
        self.model_pose = [0.0, 0.3, 0.1, 0, 0, 0, 1]
        self.obstacle_pose = [0.0, 0.0, 0.0]
        self.set_forbidden()
        self.set_init_pose()

    def set_forbidden(self):
        #set forbidden erea
        self.planning_scene = PlanningSceneInterface("base_link")
        self.planning_scene.removeCollisionObject("my_ground")
        self.planning_scene.addCube("my_ground", 2, 0, 0, -1.04)
        self.planning_scene.addBox("obstacle", 0.1, 0.5, 1, -0.3, 0, 0.5)
        self.planning_scene.addBox("obstacle2", 0.5, 0.1, 1, 0, -0.3, 0.5)
        #self.planning_scene.addCube("demo_cube", 0.06,0,0.3,0)
        print dir(self.planning_scene)
        import inspect
        print "addBox's variable is ", inspect.getargspec(
            self.planning_scene.addBox)
        print "attachBox's variable is ", inspect.getargspec(
            self.planning_scene.attachBox)
        print "addCube's variable is ", inspect.getargspec(
            self.planning_scene.addCube)
        print "setColor's variable is ", inspect.getargspec(
            self.planning_scene.setColor)

    def set_init_pose(sefl):
        #set init pose
        move_group = MoveGroupInterface("manipulator", "base_link")
        planning_scene = PlanningSceneInterface("base_link")
        joint_names = [
            "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
            "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
        ]
        pose = [-1.26, -0.64, -2.44, -0.66, 1.56, 0.007]
        move_group.moveToJointPosition(joint_names, pose, wait=False)
        move_group.get_move_action().wait_for_result()
        result = move_group.get_move_action().get_result()
        move_group.get_move_action().cancel_all_goals()

    def callback(self, data):
        x = data.pose[1].position.x
        y = data.pose[1].position.y
        z = data.pose[1].position.z

    def start_subscriber(self):
        rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback)

    def reaching_pose(self):
        print self.model_pose
        group = moveit_commander.MoveGroupCommander("manipulator")
        #print group.get_current_pose().pose
        pose_target = geometry_msgs.msg.Pose()
        pose = group.get_current_pose().pose
        pose_target = geometry_msgs.msg.Pose()
        pose_target.orientation.x = self.model_pose[3]
        pose_target.orientation.y = self.model_pose[4]
        pose_target.orientation.z = self.model_pose[5]
        pose_target.orientation.w = self.model_pose[6]
        pose_target.position.x = self.model_pose[0]
        pose_target.position.y = self.model_pose[1]
        pose_target.position.z = self.model_pose[2]

        group.set_pose_target(pose_target)
        group.go()
        #raw_input("Enter -->")

    def gripper_close(self):
        print("Gripper_Close")
        gripper_pose = Char()
        gripper_pose = 255
        pub.publish(gripper_pose)
        #raw_input("Enter -->")

    def gripper_open(self):
        print("Gripper_Open")
        gripper_pose = Char()
        gripper_pose = 0
        pub.publish(gripper_pose)
        #raw_input("Enter -->")

    def move_45(self):
        rate = rospy.Rate(1)
        rospy.sleep(1)

        a = 0.065
        b = 0.040
        c = 0.012

        pattern_45 = [
            [-0.10, 0.31, 0.015, 0, 0, 0, 1],  #B
            'c',
            [-0.10, 0.31, 0.1, 0, 0, 0, 1],  #B
            #[0.0  ,0.31   ,0.15    , 0,0,0,1],  #A90
            [0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795],
            [0, 0.5, 0.0365, 0.3826834, 0, 0, 0.9238795],
            'o',
            [0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795],
            #[0.0  ,0.31   ,0.1    , 0,0,0,1],   #A90
            [-0.10, 0.401, 0.041, 0, 0, 0, 1],
            'c',
            [-0.10, 0.401, 0.100, 0, 0, 0, 1],
            [0, 0.5 - a, 0.037 + a, 0.3826834, 0, 0, 0.9238795],
            [0, 0.5 - b, 0.037 + b, 0.3826834, 0, 0, 0.9238795],
            [0, 0.5 - c, 0.037 + c, 0.3826834, 0, 0, 0.9238795],
            'o',
            'c',
            [0, 0.5 - a, 0.037 + a, 0.3826834, 0, 0, 0.9238795],
            [-0.10, 0.401, 0.100, 0, 0, 0, 1],
            [-0.10, 0.401, 0.050, 0, 0, 0, 1],
            'o',
            [-0.10, 0.401, 0.100, 0, 0, 0, 1],
            [0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795],
            [0, 0.5, 0.0365, 0.3826834, 0, 0, 0.9238795],
            'c',
            [0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795],
            [-0.10, 0.31, 0.1, 0, 0, 0, 1],  #B
            [-0.10, 0.31, 0.015, 0, 0, 0, 1],  #B
            'o',
            [-0.10, 0.31, 0.1, 0, 0, 0, 1],  #B
        ]

        while not rospy.is_shutdown():
            for pose in pattern_45:
                if pose == 'c':
                    self.gripper_close()
                    rospy.sleep(1)
                elif pose == 'o':
                    self.gripper_open()
                    rospy.sleep(1)
                else:
                    self.model_pose = pose
                    self.reaching_pose()

                if rospy.is_shutdown():
                    return 0

    def move_90(self):
        rate = rospy.Rate(1)
        rospy.sleep(1)

        a = 0.065
        b = 0.040
        c = 0.012

        pattern_45 = [
            [-0.10, 0.31, 0.015, 0, 0, 0, 1],  #B
            'c',
            [-0.10, 0.31, 0.1, 0, 0, 0, 1],  #B
            [0.0, 0.300, 0.1, 0, 0, 0, 1],  #A90
            [0.0, 0.315, 0.0358, 0, 0, 0, 1],  #A90
            'o',
            [0.0, 0.315, 0.1, 0, 0, 0, 1],  #A90
            [-0.10, 0.40, 0.041, 0, 0, 0, 1],  #negi
            'c',
            [-0.10, 0.40, 0.1, 0, 0, 0, 1],
            [-0.001, 0.316, 0.1, 0, 0, 0, 1],  #A90
            [-0.001, 0.316, 0.053, 0, 0, 0, 1],  #A90
            'o',
            'c',
            [0.0, 0.314, 0.1, 0, 0, 0, 1],  #A90
            [-0.10, 0.40, 0.1, 0, 0, 0, 1],
            [-0.10, 0.40, 0.05, 0, 0, 0, 1],  #negi
            'o',
            #[-0.10,0.40    ,0.1    , 0,0,0,1],
            [0.0, 0.300, 0.1, 0, 0, 0, 1],  #A90
            [0.0, 0.315, 0.036, 0, 0, 0, 1],  #A90
            'c',
            [0.0, 0.300, 0.1, 0, 0, 0, 1],  #A90
            [-0.10, 0.320, 0.1, 0, 0, 0, 1],  #B
            [-0.10, 0.31, 0.0153, 0, 0, 0, 1],  #B
            'o',
            [-0.10, 0.31, 0.1, 0, 0, 0, 1],  #B
        ]

        while not rospy.is_shutdown():
            for pose in pattern_45:
                if pose == 'c':
                    self.gripper_close()
                    rospy.sleep(1)
                elif pose == 'o':
                    self.gripper_open()
                    rospy.sleep(1)
                else:
                    self.model_pose = pose
                    self.reaching_pose()

                if rospy.is_shutdown():
                    return 0
Ejemplo n.º 45
0
from moveit_msgs.msg import CollisionObject  #, AttachedCollisionObject
from shape_msgs.msg import SolidPrimitive
from math import pi, sqrt, cos, sin
from copy import deepcopy
import numpy as np
import baxter_pykdl as kdl
from tf.transformations import *

#################################### INITIALISATION ####################################
# Init node
rospy.init_node('kinect_pnp', anonymous=True)
# Baxter Kinematics
rightSolver = kdl.baxter_kinematics('right')
leftSolver = kdl.baxter_kinematics('left')
# Initialize the planning scene interface.
p = PlanningSceneInterface("base")
# Connect the arms to the move group.
g = MoveGroupInterface("both_arms", "base")
gr = MoveGroupInterface("right_arm", "base")
gl = MoveGroupInterface("left_arm", "base")
# Create baxter_interface limb instance.
rightarm = baxter_interface.limb.Limb('right')
leftarm = baxter_interface.limb.Limb('left')
# Create baxter_interface gripper instance.
rightgripper = baxter_interface.Gripper('right')
leftgripper = baxter_interface.Gripper('left')

# Constant
bskposR = PoseStamped()
bskposR.header.frame_id = "base"
bskposR.header.stamp = rospy.Time.now()
Ejemplo n.º 46
0
class Reaching:
    def __init__(self):
        rospy.init_node('reaching', anonymous=True)
        self.model_pose = [0.0, 0.0, 0.0]

        self.set_init_pose()
        #self.set_forbidden()

    def set_forbidden(self):
        #set forbidden erea
        self.planning_scene = PlanningSceneInterface("base_link")
        self.planning_scene.removeCollisionObject("my_front_ground")
        self.planning_scene.removeCollisionObject("my_back_ground")
        self.planning_scene.removeCollisionObject("my_right_ground")
        self.planning_scene.removeCollisionObject("my_left_ground")
        self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
        self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
        self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
        self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
        self.planning_scene.removeCollisionObject("demo_cube")

    def set_init_pose(self):
        #set init pose
        move_group = MoveGroupInterface("arm_with_torso", "base_link")
        #set arm initial position
        joints = [
            "torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
            "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
            "wrist_flex_joint", "wrist_roll_joint"
        ]
        #pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        pose = [0.385, -1.0, -1.0, 0.0, 1.0, 0.0, 1.5, 0.0]
        #pose = [0.385, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
        result = move_group.moveToJointPosition(joints, pose, 0.02)

    def target(self):
        rate = rospy.Rate(1)
        rospy.sleep(1)
        ls = tf.TransformListener()
        while not rospy.is_shutdown():
            print self.model_pose

            try:
                (trans, rot) = ls.lookupTransform('/base_link', '/object',
                                                  rospy.Time(0))
                print "tf base_link to test", trans

                group = moveit_commander.MoveGroupCommander("arm_with_torso")
                #print group.get_current_pose().pose
                pose_target = geometry_msgs.msg.Pose()
                pose = group.get_current_pose().pose
                pose_target = geometry_msgs.msg.Pose()
                pose_target.orientation.x = pose.orientation.x
                pose_target.orientation.y = pose.orientation.y
                pose_target.orientation.z = pose.orientation.z
                pose_target.orientation.w = pose.orientation.w
                pose_target.position.x = trans[0]
                pose_target.position.y = trans[1]
                pose_target.position.z = trans[2] + 0.3

                group.set_pose_target(pose_target)
                group.go()

            except (tf.LookupException, tf.ConnectivityException,
                    tf.ExtrapolationException):
                print "e"

            rospy.sleep(1)
Ejemplo n.º 47
0
class GraspingClient(object):
    def __init__(self):
        self.scene = PlanningSceneInterface("base_link")
        self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
        self.move_group = MoveGroupInterface("arm", "base_link")

        find_topic = "basic_grasping_perception/find_objects"
        rospy.loginfo("Waiting for %s..." % find_topic)
        self.find_client = actionlib.SimpleActionClient(
            find_topic, FindGraspableObjectsAction)
        self.find_client.wait_for_server()

    def updateScene(self):
        # find objects
        goal = FindGraspableObjectsGoal()
        goal.plan_grasps = True
        self.find_client.send_goal(goal)
        self.find_client.wait_for_result(rospy.Duration(5.0))
        find_result = self.find_client.get_result()

        # remove previous objects
        for name in self.scene.getKnownCollisionObjects():
            self.scene.removeCollisionObject(name, False)
        for name in self.scene.getKnownAttachedObjects():
            self.scene.removeAttachedObject(name, False)
        self.scene.waitForSync()

        # insert objects to scene
        idx = -1
        for obj in find_result.objects:
            idx += 1
            obj.object.name = "object%d" % idx
            self.scene.addSolidPrimitive(obj.object.name,
                                         obj.object.primitives[0],
                                         obj.object.primitive_poses[0],
                                         wait=False)

        for obj in find_result.support_surfaces:
            # extend surface to floor, and make wider since we have narrow field of view
            height = obj.primitive_poses[0].position.z
            obj.primitives[0].dimensions = [
                obj.primitives[0].dimensions[0],
                1.5,  # wider
                obj.primitives[0].dimensions[2] + height
            ]
            obj.primitive_poses[0].position.z += -height / 2.0

            # add to scene
            self.scene.addSolidPrimitive(obj.name,
                                         obj.primitives[0],
                                         obj.primitive_poses[0],
                                         wait=False)

        self.scene.waitForSync()

        # store for grasping
        self.objects = find_result.objects
        self.surfaces = find_result.support_surfaces

    def getGraspableCube(self):
        graspable = None
        for obj in self.objects:
            # need grasps
            if len(obj.grasps) < 1:
                continue
            # check size
            if obj.object.primitives[0].dimensions[0] < 0.05 or \
               obj.object.primitives[0].dimensions[0] > 0.07 or \
               obj.object.primitives[0].dimensions[0] < 0.05 or \
               obj.object.primitives[0].dimensions[0] > 0.07 or \
               obj.object.primitives[0].dimensions[0] < 0.05 or \
               obj.object.primitives[0].dimensions[0] > 0.07:
                continue
            # has to be on table
            if obj.object.primitive_poses[0].position.z < 0.5:
                continue
            return obj.object, obj.grasps
        # nothing detected
        return None, None

    def getSupportSurface(self, name):
        for surface in self.support_surfaces:
            if surface.name == name:
                return surface
        return None

    def getPlaceLocation(self):
        pass

    def pick(self, block, grasps):
        success, pick_result = self.pickplace.pick_with_retry(
            block.name,
            grasps,
            support_name=block.support_surface,
            scene=self.scene)
        self.pick_result = pick_result
        return success

    def place(self, block, pose_stamped):
        places = list()
        l = PlaceLocation()
        l.place_pose.pose = pose_stamped.pose
        l.place_pose.header.frame_id = pose_stamped.header.frame_id

        # copy the postrection
        m = 16  # number of possible place poses
        pi = 3.141592653589
        for i in range(0, m - 1):
            l.place_pose.pose = rotate_pose_msg_by_euler_angles(
                l.place_pose.pose, 0, 0, 2 * pi / m)
            places.append(copy.deepcopy(l))

        success, place_result = self.pickplace.place_with_retry(
            block.name, places, scene=self.scene)
        return success

    def tuck(self):
        joints = [
            "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
            "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
            "wrist_roll_joint"
        ]
        pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
        while not rospy.is_shutdown():
            result = self.move_group.moveToJointPosition(joints, pose, 0.02)
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                return
Ejemplo n.º 48
0
class GripperInterfaceClient:
    def __init__(self):
        self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"]

        self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction)
        rospy.loginfo("Waiting for gripper_controller...")
        self.client.wait_for_server()

        self.scene = PlanningSceneInterface("base_link")
        self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True)

        find_topic = "basic_grasping_perception/find_objects"
        rospy.loginfo("Waiting for %s..." % find_topic)
        self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
        self.find_client.wait_for_server()

    def execute_trajectory(self, trajectory):
        goal_position = trajectory.points[-1].positions
        print "==========", goal_position
        command_goal = GripperCommandGoal()
        command_goal.command.position = goal_position[0] + goal_position[1]
        command_goal.command.max_effort = 50.0  # Placeholder. TODO Figure out a better way to compute this
        self.client.send_goal(command_goal)
        self.client.wait_for_result()

    def move_to(self, goal_position, max_effort):
        command_goal = GripperCommandGoal()
        command_goal.command.position = goal_position
        command_goal.command.max_effort = max_effort
        self.client.send_goal(command_goal)
        self.client.wait_for_result()

    def updateScene(self):
        # find objects
        goal = FindGraspableObjectsGoal()
        goal.plan_grasps = True
        self.find_client.send_goal(goal)
        self.find_client.wait_for_result(rospy.Duration(5.0))
        find_result = self.find_client.get_result()

        # remove previous objects
        for name in self.scene.getKnownCollisionObjects():
            self.scene.removeCollisionObject(name, False)
        for name in self.scene.getKnownAttachedObjects():
            self.scene.removeAttachedObject(name, False)
        self.scene.waitForSync()

        # insert objects to scene
        idx = -1
        for obj in find_result.objects:
            idx += 1
            obj.object.name = "object%d" % idx
            self.scene.addSolidPrimitive(
                obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait=False
            )
        # Try mesh representation
        # idx = -1
        # for obj in find_result.objects:
        #    idx += 1
        #    obj.object.name = "object%d"%idx
        #    self.scene.addMesh(obj.object.name,
        #                       obj.object.mesh_poses[0],
        #                       obj.object.meshes[0],
        #                       wait = False)

        for obj in find_result.support_surfaces:
            # extend surface to floor, and make wider since we have narrow field of view
            height = obj.primitive_poses[0].position.z
            obj.primitives[0].dimensions = [
                obj.primitives[0].dimensions[0] + 0.1,
                2.5,  # wider
                obj.primitives[0].dimensions[2] + height,
            ]
            obj.primitive_poses[0].position.z += -height / 2.0

            # add to scene
            self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait=False)

        self.scene.waitForSync()

        # store for grasping
        self.objects = find_result.objects
        self.surfaces = find_result.support_surfaces

    def getGraspableCube(self):
        graspable = None
        for obj in self.objects:
            # need grasps
            if len(obj.grasps) < 1:
                continue
            # check size
            # if obj.object.primitives[0].dimensions[0] < 0.03 or \
            #   obj.object.primitives[0].dimensions[0] > 0.04 or \
            #   obj.object.primitives[0].dimensions[0] < 0.07 or \
            #   obj.object.primitives[0].dimensions[0] > 0.09 or \
            #   obj.object.primitives[2].dimensions[2] < 0.19 or \
            #   obj.object.primitives[2].dimensions[2] > 0.20:
            #    continue
            # has to be on table
            # if obj.object.primitive_poses[0].position.z < 0.5:
            #    continue
            return obj.object, obj.grasps
        # nothing detected
        return None, None

    def plan_pick(self, block, grasps):
        success, pick_result = self.pickplace.pick_with_retry(
            block.name,
            grasps,
            support_name=block.support_surface,
            allow_gripper_support_collision=False,
            planner_id="PRMkConfigDefault",
            retries=2,
            scene=self.scene,
        )
        # self.pick_result = pick_result
        if success:
            return pick_result.trajectory_stages
        else:
            rospy.loginfo("Planning Failed.")
            return None
Ejemplo n.º 49
0
    def __init__(self, sim=False):
        # Define planning groups
        self.scene = PlanningSceneInterface("base_link")
        self.dof = rospy.get_param('~jaco_dof')
        self.move_group = MoveGroupInterface("upper_body", "base_link")
        self.rmove_group = MoveGroupInterface("right_arm", "base_link")

        planner_id = "RRTConnectkConfigDefault"
        self.move_group.setPlannerId(planner_id)
        self.rmove_group.setPlannerId(planner_id)

        self.objects_heights = [0.122, 0.240]
        self.objects_heights_tolerances = [0.02, 0.03]

        # Define joints and positions for 6 and 7 DOF
        if "6dof" == self.dof:
            self._upper_body_joints = [
                "right_elbow_joint", "right_shoulder_lift_joint",
                "right_shoulder_pan_joint", "right_wrist_1_joint",
                "right_wrist_2_joint", "right_wrist_3_joint",
                "left_elbow_joint", "left_shoulder_lift_joint",
                "left_shoulder_pan_joint", "left_wrist_1_joint",
                "left_wrist_2_joint", "left_wrist_3_joint", "linear_joint",
                "pan_joint", "tilt_joint"
            ]
            self._right_arm_joints = [
                "right_elbow_joint", "right_shoulder_lift_joint",
                "right_shoulder_pan_joint", "right_wrist_1_joint",
                "right_wrist_2_joint", "right_wrist_3_joint"
            ]
            self._left_arm_joints = [
                "left_elbow_joint", "left_shoulder_lift_joint",
                "left_shoulder_pan_joint", "left_wrist_1_joint",
                "left_wrist_2_joint", "left_wrist_3_joint"
            ]
            self.tucked = [
                -2.8, -1.48, -1.48, 0, 0, 1.571, 2.8, 1.48, 1.48, 0, 0, -1.571,
                0.0371, 0.0, 0.0
            ]
            self.constrained_stow = [
                2.28, 2.00, -2.56, -0.09, 0.15, 1.082, -2.28, -2.17, 2.56,
                0.09, -0.15, 2.06, 0.42, 0.0, 0.0
            ]
            self.rarm_const_stow = [2.28, 2.00, -2.56, -0.09, 0.15, 1.06]
            #self.rarm_const_stow = [-1.51, -0.22, -2.00, -2.04, 1.42, 1.32]

        elif "7dof" == self.dof:
            self._upper_body_joints = [
                "right_shoulder_pan_joint", "right_shoulder_lift_joint",
                "right_arm_half_joint", "right_elbow_joint",
                "right_wrist_spherical_1_joint",
                "right_wrist_spherical_2_joint", "right_wrist_3_joint",
                "left_shoulder_pan_joint", "left_shoulder_lift_joint",
                "left_arm_half_joint", "left_elbow_joint",
                "left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
                "left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint"
            ]
            self._right_arm_joints = [
                "right_shoulder_pan_joint", "right_shoulder_lift_joint",
                "right_arm_half_joint", "right_elbow_joint",
                "right_wrist_spherical_1_joint",
                "right_wrist_spherical_2_joint", "right_wrist_3_joint"
            ]
            self._left_arm_joints = [
                "left_shoulder_pan_joint", "left_shoulder_lift_joint",
                "left_arm_half_joint", "left_elbow_joint",
                "left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
                "left_wrist_3_joint"
            ]
            self.tucked = [
                -1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7,
                0.0, -0.5, 1.7, 0.04, 0, 0
            ]
            self.constrained_stow = [
                -2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0,
                0.0, -1.0, 0.42, 0, 0
            ]
            self.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0]

        else:
            rospy.logerr("DoF needs to be set 6 or 7, aborting demo")
            return

        # Define the MoveIt pickplace pipeline
        self.pickplace = None
        self.pickplace = PickPlaceInterface("left_side",
                                            "left_gripper",
                                            verbose=True)
        self.pickplace.planner_id = planner_id
        self.pick_result = None
        self.place_result = None

        self._listener = tf.TransformListener()

        self._lgripper = GripperActionClient('left')

        find_grasp_planning_topic = "grasp_planner/plan"
        rospy.loginfo("Waiting for %s..." % find_grasp_planning_topic)
        self.find_grasp_planning_client = actionlib.SimpleActionClient(
            find_grasp_planning_topic, GraspPlanningAction)
        self.find_grasp_planning_client.wait_for_server()
        rospy.loginfo("...connected")

        self.scene.clear()

        # This is a simulation so need to adjust gripper parameters
        if sim:
            self._gripper_closed = 0.96
            self._gripper_open = 0.00
        else:
            self._gripper_closed = 0.01
            self._gripper_open = 0.165
Ejemplo n.º 50
0
class GraspingClient(object):

    def __init__(self):
        self.scene = PlanningSceneInterface("base_link")
        self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
        self.move_group = MoveGroupInterface("arm", "base_link")

        find_topic = "basic_grasping_perception/find_objects"
        rospy.loginfo("Waiting for %s..." % find_topic)
        self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
        self.find_client.wait_for_server()

    def updateScene(self):
        # find objects
	rospy.loginfo("Add collision objects")

        self.scene.removeCollisionObject("my_front_ground")
        self.scene.removeCollisionObject("my_back_ground")
        self.scene.removeCollisionObject("my_right_ground")
        self.scene.removeCollisionObject("my_left_ground")
        self.scene.removeCollisionObject("box_1")
        self.scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
        self.scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
        self.scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
        self.scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
        self.scene.addBox("box_1", 0.44, 0.43, 0.68, 0.7, -0.4, .34)
        self.scene.waitForSync()

    def tuck(self):
        joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
                  "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
        pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
        while not rospy.is_shutdown():
            result = self.move_group.moveToJointPosition(joints, pose, 0.02)
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                return

    def zero(self):
        joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
                  "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
        pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        while not rospy.is_shutdown():
            result = self.move_group.moveToJointPosition(joints, pose, 0.02)
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                return