Пример #1
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")
Пример #2
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
Пример #3
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
Пример #4
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
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
Пример #6
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)
Пример #7
0
    def __init__(self):

        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('moveit_demo')

        ptu_action = FollowTrajectoryClient(
            "ptu_controller", ["ptu_pan_joint", "ptu_tilt_joint"])
        ptu_action.move_to([
            0.0,
            -0.75,
        ])

        # Use the planning scene object to add or remove objects
        scene = PlanningSceneInterface(REFERENCE_FRAME)

        # 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()
        print("End Effector Link", end_effector_link)

        # Allow some leeway in position (meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.02)
        right_arm.set_goal_orientation_tolerance(0.05)

        # 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 15 seconds per planning attempt
        right_arm.set_planning_time(15)

        # Set a limit on the number of pick attempts before bailing
        max_pick_attempts = 10

        # 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
        find_topic = "basic_grasping_perception/find_objects"
        sub_topic = "handles_position"
        rospy.Subscriber(sub_topic, target_pose, getHandlesCb)
        rospy.loginfo(
            "Connecting to basic_grasping_perception/find_objects...")
        find_objects = actionlib.SimpleActionClient(
            find_topic, 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('right_down')
        # 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(10.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))

            rospy.loginfo("Found %d Support Surfaces" %
                          len(find_result.support_surfaces))

            # print("perception_results_primitives",find_result.objects.primitives[0])
            # print("perception_results_primitives_poses",find_result.objects.primitives_poses)

            # 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
            # target_position = None
            the_object = None
            the_object_dist = 1.0
            count = -1
            # target_size
            # 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
                    print("target_size %d ", count, target_size)
                    # target_position = obj.object.primitive_poses[0].position
                    # print("target_size %d ", %count, target_size )

                    # Set the target pose
                    target_pose.pose = obj.object.primitive_poses[0]
                    print("target_pose_before ", target_pose)

                    # We want the gripper to be horizontal
                    target_pose.pose.orientation.x = 0.519617092464
                    target_pose.pose.orientation.y = -0.510439243162
                    target_pose.pose.orientation.z = 0.479912175114
                    target_pose.pose.orientation.w = -0.489013456294

                    print("target_pose_after ", target_pose)

                # 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] ]
                # obj.primitive_poses[0].position.z += -height/1.5

                # 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
                print("table_size", table_size)

            # 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]
                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)
Пример #8
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")

        grasp_topic = "fetch_grasp_planner_node/plan"
        rospy.loginfo("Waiting for %s..." % grasp_topic)
        self.grasp_planner_client = actionlib.SimpleActionClient(
            grasp_topic, GraspPlanningAction)
        wait = self.grasp_planner_client.wait_for_server(rospy.Duration(5))
        if (wait):
            print("successfully connected to grasp server")
        else:
            print("failed to connect to grasp server")

        rospy.Subscriber("fetch_fruit_harvest/apple_pose", Pose,
                         self.apple_pose_callback)
        self.pub = rospy.Publisher("fetch_fruit_harvest/grasp_msg",
                                   String,
                                   queue_size=10)
        self.object = Object()
        self.grasps = Grasp()
        self.ready_for_grasp = False
        self.pick_place_finished = False
        self.first_time_grasp = True
        self.scene.addBox("ground", 300, 300, 0.02, 0, 0, 0)
        self.tuck()

    def apple_pose_callback(self, message):
        if self.pick_place_finished or self.first_time_grasp:
            self.first_time_grasp = False
            self.pick_place_finished = False
            print("apple_pose_callback")
            apple = SolidPrimitive()
            apple.type = SolidPrimitive.SPHERE
            apple.dimensions = [0.03, 0.03, 0.03]
            self.object.name = "apple"
            self.object.primitives = [apple]
            self.object.primitive_poses = [message]
            # add stamp and frame
            self.object.header.stamp = rospy.Time.now()
            self.object.header.frame_id = "base_link"

            goal = GraspPlanningGoal()
            goal.object = self.object
            self.grasp_planner_client.send_goal(goal)
            self.grasp_planner_client.wait_for_result(rospy.Duration(5.0))
            grasp_planner_result = self.grasp_planner_client.get_result()
            self.grasps = grasp_planner_result.grasps
            self.ready_for_grasp = True

    def updateScene(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()

        # insert objects to scene
        self.scene.addBox("ground", 300, 300, 0.02, 0, 0, 0)
        self.scene.addSolidPrimitive(self.object.name,
                                     self.object.primitives[0],
                                     self.object.primitive_poses[0])
        self.scene.waitForSync()

    def getPlaceLocation(self):
        pass

    def pick(self, block, grasps):
        success, pick_result = self.pickplace.pick_with_retry(block.name,
                                                              grasps,
                                                              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
    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)
Пример #10
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
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")
        self.objectname_sub = rospy.Subscriber("/objects", Float32MultiArray,
                                               self.callback)
        self.interestedObjectName = ""
        #self.objects = []

        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 callback(self, msg):
        if len(msg.data) != 0:
            self.interestedObjectName = "/object_" + (str(int(msg.data[0])))
            #print(self.interestedObjectName)

    def getInterestedObject(self, objects):
        listener = tf.TransformListener()
        while not rospy.is_shutdown():
            try:
                if len(self.interestedObjectName) == 0:
                    continue
                (trans,
                 rot) = listener.lookupTransform("/base_link",
                                                 self.interestedObjectName,
                                                 rospy.Time(0))
                if len(trans) == 3:
                    break
            except (tf.LookupException, tf.ConnectivityException,
                    tf.ExtrapolationException):
                continue
        final_objects = []
        for obj in objects:
            print(obj[0].object.name)
            print("obj_x: " + str(trans[0]) + "----" +
                  str(obj[0].object.primitive_poses[0].position.x))
            print("obj_x: " + str(trans[1]) + "----" +
                  str(obj[0].object.primitive_poses[0].position.y))
            print("obj_x: " + str(trans[2]) + "----" +
                  str(obj[0].object.primitive_poses[0].position.z))
            if (abs(obj[0].object.primitive_poses[0].position.x - trans[0]) <=
                    0.1 and abs(obj[0].object.primitive_poses[0].position.y -
                                trans[1]) <= 0.1
                    and abs(obj[0].object.primitive_poses[0].position.z -
                            trans[2]) <= 0.1):
                return [obj[0]]
                break
        return []
        rospy.loginfo("No Interested Objects found")

    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
        objects = list()
        idx = -1

        for obj in find_result.objects:
            idx += 1
            obj.object.name = "object%d" % idx
            temp_height = obj.object.primitive_poses[0].position.z
            obj.object.primitive_poses[0].position.z -= 0.02 * temp_height
            self.scene.addSolidPrimitive(obj.object.name,
                                         obj.object.primitives[0],
                                         obj.object.primitive_poses[0],
                                         wait=False)
            if obj.object.primitive_poses[0].position.x < 0.85:
                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
            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

        # store graspable objects by Z
        #objects.sort(key=lambda object: object[1])
        #objects.reverse()
        #self.objects = [object[0] for object in objects]
        self.objects = self.getInterestedObject(objects)
        #for object in objects:
        #    print(object[0].object.name, object[1])
        #exit(-1)

    def getGraspableObject(self):
        graspable = None
        if len(self.objects) == 0:
            return None, 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.03 or \
               obj.object.primitives[0].dimensions[0] < 0.03:
                continue
            # has to be on table
            if obj.object.primitive_poses[0].position.z < 0.5:
                continue
            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,
            support_name=block.support_surface,
            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
Пример #12
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()
Пример #13
0
class MoveItDemo:
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)
        
        rospy.init_node('moveit_demo')

         # We need a tf2 listener to convert poses into arm reference base
        try:
            self._tf2_buff = tf2_ros.Buffer()
            self._tf2_list = tf2_ros.TransformListener(self._tf2_buff)
        except rospy.ROSException as err:
            rospy.logerr("MoveItDemo: could not start tf buffer client: " + str(err))
            raise err

        self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening") - 0.01]
        self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening") + 0.01]
        self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral",
                                                (self.gripper_opened[0] + self.gripper_closed[0])/2.0) ]
        
        self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten", 0.0) 
                
        # Use the planning scene object to add or remove objects
        self.scene = PlanningSceneInterface(REFERENCE_FRAME)
        
        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)
        
        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('target_pose', PoseStamped, queue_size=5)
        
        # Create a dictionary to hold object colors
        self.colors = dict()
                        
        # Initialize the move group for the right arm
        arm = MoveGroupCommander(GROUP_NAME_ARM)
        
        # Initialize the move group for the right gripper
        gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
        
        # Get the name of the end-effector link
        end_effector_link = arm.get_end_effector_link()
 
        # Allow some leeway in position (meters) and orientation (radians)
        arm.set_goal_position_tolerance(0.05)
        arm.set_goal_orientation_tolerance(0.1)

        # Allow replanning to increase the odds of a solution
        arm.allow_replanning(True)
        
        # Set the right arm reference frame
        arm.set_pose_reference_frame(REFERENCE_FRAME)
        
        # Allow 5 seconds per planning attempt
        arm.set_planning_time(15)
        
        # Set a limit on the number of pick attempts before bailing
        max_pick_attempts = 1
        
        # 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 simple_grasping 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
        arm.set_named_target('right_up')
        if arm.go() != True:
            rospy.logwarn("  Go failed")

        gripper.set_joint_value_target(self.gripper_opened)
        if gripper.go() != True:
            rospy.logwarn("  Go failed")
       
        rospy.sleep(1)
        
        while not rospy.is_shutdown():
            # Initialize the grasping goal
            goal = FindGraspableObjectsGoal()
            
            # We don't use the simple_grasping 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 self.scene.getKnownCollisionObjects():
                self.scene.removeCollisionObject(name, False)
            for name in self.scene.getKnownAttachedObjects():
                self.scene.removeAttachedObject(name, False)
            self.scene.waitForSync()
            
            # Clear the virtual object colors
            self.scene._colors = dict()

            # Use the nearest object on the table as the target
            target_pose = PoseStamped()
            target_pose.header.frame_id = REFERENCE_FRAME
            target_id = 'target'
            target_size = None
            the_object = None
            the_object_dist = 0.30
            the_object_dist_min = 0.10
            count = -1
            
            for obj in find_result.objects:
                count += 1
                self.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
                dy = obj.object.primitive_poses[0].position.y
                d = math.sqrt((dx * dx) + (dy * dy))

                if d < the_object_dist and d > the_object_dist_min:
                    rospy.loginfo("object is in the working zone")

                    the_object_dist = d
                    the_object = count

                    target_size = [0.02, 0.02, 0.05]
                    
                    target_pose = PoseStamped()
                    target_pose.header.frame_id = REFERENCE_FRAME

                    target_pose.pose.position.x = obj.object.primitive_poses[0].position.x + target_size[0] / 2.0
                    target_pose.pose.position.y = obj.object.primitive_poses[0].position.y 
                    target_pose.pose.position.z = 0.04

                    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
                else:
                    rospy.loginfo("object is not in the working zone")
                    rospy.sleep(1)
                
                # 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
                self.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
            self.scene.waitForSync()

            # Set colors of the table and the object we are grabbing
            self.scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0)  # orange
            self.scene.setColor(find_result.objects[the_object].object.support_surface, 0.3, 0.3, 0.3, 0.7)  # grey
            self.scene.sendColors()

            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
            arm.set_support_surface_name(support_surface)
                            
            # 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")

            # Generate a list of grasps
            grasps = self.make_grasps(grasp_pose, [target_id], [target_size[1] - self.gripper_tighten])

            # 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
            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 = arm.pick(target_id, grasps)
                n_attempts += 1
                rospy.loginfo("Pick attempt: " +  str(n_attempts))
                rospy.sleep(1.0)
                
            
            rospy.sleep(2)
            
            # Open the gripper to the neutral position
            gripper.set_joint_value_target(self.gripper_neutral)
            gripper.go()
            
            rospy.sleep(2)
            
            # Return the arm to the "resting" pose stored in the SRDF file
            arm.set_named_target('right_up')
            arm.go()
                
            rospy.sleep(2)
            
            if args.once:
                moveit_commander.roscpp_shutdown()
                
                # Exit the script
                moveit_commander.os._exit(0)
        
    # Get the gripper posture as a JointTrajectory
    def make_gripper_posture(self, joint_positions):
        # Initialize the joint trajectory for the gripper joints
        t = JointTrajectory()
        
        # Set the joint names to the gripper joint names
        t.header.stamp = rospy.get_rostime()
        t.joint_names = GRIPPER_JOINT_NAMES
        
        # Initialize a joint trajectory point to represent the goal
        tp = JointTrajectoryPoint()
        
        # Assign the trajectory joint positions to the input positions
        tp.positions = joint_positions
        
        # Set the gripper effort
        tp.effort = GRIPPER_EFFORT
        
        tp.time_from_start = rospy.Duration(1.0)
        
        # Append the goal point to the trajectory points
        t.points.append(tp)
        
        # Return the joint trajectory
        return t
    
    # Generate a gripper translation in the direction given by vector
    def make_gripper_translation(self, min_dist, desired, vector):
        # Initialize the gripper translation object
        g = GripperTranslation()
        
        # Set the direction vector components to the input
        g.direction.vector.x = vector[0]
        g.direction.vector.y = vector[1]
        g.direction.vector.z = vector[2]
        
        # The vector is relative to the gripper frame
        g.direction.header.frame_id = GRIPPER_FRAME
        
        # Assign the min and desired distances from the input
        g.min_distance = min_dist
        g.desired_distance = desired
        
        return g

    # Generate a list of possible grasps
    def make_grasps(self, initial_pose_stamped, allowed_touch_objects, grasp_opening=[0]):
        # Initialize the grasp object
        g = Grasp()

        # Set the pre-grasp and grasp postures appropriately;
        # grasp_opening should be a bit smaller than target width
        g.pre_grasp_posture = self.make_gripper_posture(self.gripper_opened)
        g.grasp_posture = self.make_gripper_posture(grasp_opening)

        # Set the approach and retreat parameters as desired
        g.pre_grasp_approach = self.make_gripper_translation(0.1, 0.15, [0.0, 0.0, -1.5])
        g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.15, [0.0, 0.0, 1.5])

        # Set the first grasp pose to the input pose
        g.grasp_pose = initial_pose_stamped

        # Pitch angles to try
        pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4, 0.5, -0.5, 0.6, -0.6, 0.7, -0.7, 0.8, -0.8, 0.9, -0.9, 1.0, -1.0, 1.1, -1.1, 1.2, -1.2, 1.3, -1.3, 1.4, -1.4, 1.5, -1.5]

        # Yaw angles to try; given the limited dofs of turtlebot_arm, we must calculate the heading
        # from arm base to the object to pick (first we must transform its pose to arm base frame)
        target_pose_arm_ref = self._tf2_buff.transform(initial_pose_stamped, ARM_BASE_FRAME)
        x = target_pose_arm_ref.pose.position.x
        y = target_pose_arm_ref.pose.position.y
        yaw_vals = [atan2(y, x) + inc for inc in [0, 0.1,-0.1]]

        # A list to hold the grasps
        grasps = []

        # Generate a grasp for each pitch and yaw angle
        for yaw in yaw_vals:
            for pitch in pitch_vals:
                # Create a quaternion from the Euler angles
                q = quaternion_from_euler(0, pitch, yaw)

                # Set the grasp pose orientation accordingly
                g.grasp_pose.pose.orientation.x = q[0]
                g.grasp_pose.pose.orientation.y = q[1]
                g.grasp_pose.pose.orientation.z = q[2]
                g.grasp_pose.pose.orientation.w = q[3]

                # Set and id for this grasp (simply needs to be unique)
                g.id = str(len(grasps))

                # Set the allowed touch objects to the input list
                g.allowed_touch_objects = allowed_touch_objects

                # Don't restrict contact force
                g.max_contact_force = 0

                # Degrade grasp quality for increasing pitch angles
                g.grasp_quality = 1.0 - abs(pitch)

                # Append the grasp to the list
                grasps.append(deepcopy(g))

        # Return the list
        return grasps
    
    # Generate a list of possible place poses
    def make_places(self, init_pose):
        # Initialize the place location as a PoseStamped message
        place = PoseStamped()

        # Start with the input place pose
        place = init_pose

        # A list of x shifts (meters) to try
        x_vals = [0, 0.005, -0.005] #, 0.01, -0.01, 0.015, -0.015]

        # A list of y shifts (meters) to try
        y_vals = [0, 0.005, -0.005, 0.01, -0.01] #, 0.015, -0.015]

        # A list of pitch angles to try
        pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4, 0.5, -0.5, 0.6, -0.6, 0.7, -0.7, 0.8, -0.8, 0.9, -0.9, 1.0, -1.0, 1.1, -1.1, 1.2, -1.2, 1.3, -1.3, 1.4, -1.4, 1.5, -1.5] #, 0.005, -0.005, 0.01, -0.01, 0.02, -0.02]

        # A list to hold the places
        places = []

        # Generate a place pose for each angle and translation
        for pitch in pitch_vals:
            for dy in y_vals:
                for dx in x_vals:
                    place.pose.position.x = init_pose.pose.position.x + dx
                    place.pose.position.y = init_pose.pose.position.y + dy
    
                    # Yaw angle: given the limited dofs of turtlebot_arm, we must calculate the heading from
                    # arm base to the place location (first we must transform its pose to arm base frame)
                    target_pose_arm_ref = self._tf2_buff.transform(place, ARM_BASE_FRAME)
                    x = target_pose_arm_ref.pose.position.x
                    y = target_pose_arm_ref.pose.position.y
                    yaw = atan2(y, x)
    
                    # Create a quaternion from the Euler angles
                    q = quaternion_from_euler(0, pitch, yaw)
    
                    # Set the place pose orientation accordingly
                    place.pose.orientation.x = q[0]
                    place.pose.orientation.y = q[1]
                    place.pose.orientation.z = q[2]
                    place.pose.orientation.w = q[3]

                    # MoveGroup::place will transform the provided place pose with the attached body pose, so the object retains
                    # the orientation it had when picked. However, with our 4-dofs arm this is infeasible (nor we care about the
                    # objects orientation!), so we cancel this transformation. It is applied here:
                    # https://github.com/ros-planning/moveit_ros/blob/jade-devel/manipulation/pick_place/src/place.cpp#L64
                    # More details on this issue: https://github.com/ros-planning/moveit_ros/issues/577
                    acobjs = self.scene.get_attached_objects([target_id])
                    aco_pose = self.get_pose(acobjs[target_id])
                    if aco_pose is None:
                        rospy.logerr("Attached collision object '%s' not found" % target_id)
                        return None

                    aco_tf = self.pose_to_mat(aco_pose)
                    place_tf = self.pose_to_mat(place.pose)
                    place.pose = self.mat_to_pose(place_tf, aco_tf)
                    rospy.logdebug("Compensate place pose with the attached object pose [%s]. Results: [%s]" \
                                   % (aco_pose, place.pose))

                    # Append this place pose to the list
                    places.append(deepcopy(place))

        # Return the list
        return places
    
    # Set the color of an object
    def setColor(self, name, r, g, b, a = 0.9):
        # Initialize a MoveIt color object
        color = ObjectColor()
        
        # Set the id to the name given as an argument
        color.id = name
        
        # Set the rgb and alpha values given as input
        color.color.r = r
        color.color.g = g
        color.color.b = b
        color.color.a = a
        
        # Update the global color dictionary
        self.colors[name] = color

    # Actually send the colors to MoveIt!
    def sendColors(self):
        # Initialize a planning scene object
        p = PlanningScene()

        # Need to publish a planning scene diff        
        p.is_diff = True
        
        # Append the colors from the global color dictionary 
        for color in self.colors.values():
            p.object_colors.append(color)
        
        # Publish the scene diff
        self.scene_pub.publish(p)

    def get_pose(self, co):
        # We get object's pose from the mesh/primitive poses; try first with the meshes
        if isinstance(co, CollisionObject):
            if co.mesh_poses:
                return co.mesh_poses[0]
            elif co.primitive_poses:
                return co.primitive_poses[0]
            else:
                rospy.logerr("Collision object '%s' has no mesh/primitive poses" % co.id)
                return None
        elif isinstance(co, AttachedCollisionObject):
            if co.object.mesh_poses:
                return co.object.mesh_poses[0]
            elif co.object.primitive_poses:
                return co.object.primitive_poses[0]
            else:
                rospy.logerr("Attached collision object '%s' has no mesh/primitive poses" % co.id)
                return None
        else:
            rospy.logerr("Input parameter is not a collision object")
            return None

    def pose_to_mat(self, pose):
        '''Convert a pose message to a 4x4 numpy matrix.

        Args:
            pose (geometry_msgs.msg.Pose): Pose rospy message class.

        Returns:
            mat (numpy.matrix): 4x4 numpy matrix
        '''
        quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
        pos = np.matrix([pose.position.x, pose.position.y, pose.position.z]).T
        mat = np.matrix(quaternion_matrix(quat))
        mat[0:3, 3] = pos
        return mat

    def mat_to_pose(self, mat, transform=None):
        '''Convert a homogeneous matrix to a Pose message, optionally premultiply by a transform.

        Args:
            mat (numpy.ndarray): 4x4 array (or matrix) representing a homogenous transform.
            transform (numpy.ndarray): Optional 4x4 array representing additional transform

        Returns:
            pose (geometry_msgs.msg.Pose): Pose message representing transform.
        '''
        if transform != None:
            mat = np.dot(mat, transform)
        pose = Pose()
        pose.position.x = mat[0,3]
        pose.position.y = mat[1,3]
        pose.position.z = mat[2,3]
        quat = quaternion_from_matrix(mat)
        pose.orientation.x = quat[0]
        pose.orientation.y = quat[1]
        pose.orientation.z = quat[2]
        pose.orientation.w = quat[3]
        return pose
Пример #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
Пример #15
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
Пример #16
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()
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
Пример #18
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
Пример #19
0
def picknplace():
    # 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.
    leftarm = baxter_interface.limb.Limb('left')
    # Create baxter_interface gripper instance.
    leftgripper = baxter_interface.Gripper('left')
    leftgripper.calibrate()
    leftgripper.open()

    # Define the joints for the positions.
    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.4580487388850858, 1.627553615946424, 0.07669903939427068, -0.3539660668045592, -1.9155585088719105, -1.4124128104454947, -0.6438884357149024,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.4580487388850858, 1.627553615946424, 0.07669903939427068, -0.3539660668045592, -1.9155585088719105, -1.4124128104454947, -0.6438884357149024]
    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]
    placegoal = PoseStamped() 
    placegoal.header.frame_id = "base"
    placegoal.header.stamp = rospy.Time.now()
    placegoal.pose.position.x = 0.55
    placegoal.pose.position.y = 0.28
    placegoal.pose.position.z = 0
    placegoal.pose.orientation.x = 1.0
    placegoal.pose.orientation.y = 0.0
    placegoal.pose.orientation.z = 0.0
    placegoal.pose.orientation.w = 0.0
    # Define variables.
    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
    # The distance from the zero point in Moveit to the ground is 0.903 m.
    # The value is not allways the same. (look in Readme)
    center_z_cube= -offset_zero_point+table_size_z+0.0275/2
    pressure_ok=0
    j=0
    k=0
    start=1
    locs_x = []
    # Initialize a list for the objects and the stacked cubes.
    objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']
    boxlist= ['box01', 'box02', 'box03', 'box04', 'box05', 'box06', 'box07', 'box08', 'box09', 'box10', 'box11']
    # Clear planning scene.
    p.clear()
    # Add table as attached object.
    p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal'])
    # Move both arms to start state where the right camera looks for objects.             
    g.moveToJointPosition(jts_both, pos1,  max_velocity_scaling_factor = 1, plan_only=False)
    time.sleep(0.5)
    # cProfile to measure the performance (time) of the task.
    pr = cProfile.Profile()
    pr.enable()
    # Loop to continue pick and place until all objects are cleared from table.
    while locs_x or start:
        # Only for the start.
	if start:
            start = 0		

        # Receive the data from all objects from the topic "detected_objects".
        temp = rospy.wait_for_message("detected_objects", PoseArray) 
        locs = temp.poses 
        locs_x = []
        locs_y = []
        orien = []
        size = []

        # Adds the data from the objects.
        for i in range(len(locs)):
            locs_x.append(locs[i].position.x) 
            locs_y.append(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 locations for same objects.
        ind_rmv = []
        for i in range(0,len(locs) ):
            if (locs_y[i] > 0.24 or locs_x[i] > 0.75):
                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)

        # Do the task only if there are still objects on the table.
        if locs_x: 
            # Clear planning scene.
	    p.clear() 
            # Add table as attached object.
            p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal'])
            # Sort objects based on size (largest first to smallest last). This was done to enable stacking large cubes.
            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])
	    # Initialize the data of the biggest object on the table.
	    xn = locs_x[0]
	    yn = locs_y[0]	
	    zn = -0.16
	    thn = orien[0]
	    sz = size[0]
	    if thn > pi/4:
	        thn = -1*(thn%(pi/4))

	    # Add the detected objects into the planning scene.
	    #for i in range(1,len(locs_x)):
	        #p.addBox(objlist[i], 0.05, 0.05, 0.0275, locs_x[i], locs_y[i], center_z_cube)
	    # Add the stacked objects as collision objects into the planning scene to avoid moving against them.
	    #for e in range(0, k):
	        #p.attachBox(boxlist[e], 0.05, 0.05, 0.0275, placegoal.pose.position.x, placegoal.pose.position.y, center_z_cube+0.0275*(e-1), 'base', touch_links=['cubes'])  
            if k>0:
	        p.attachBox(boxlist[0], 0.07, 0.07, 0.0275*k, placegoal.pose.position.x, placegoal.pose.position.y, center_z_cube, 'base', touch_links=['cubes'])   
	    p.waitForSync()
            # Move both arms to the second position.
            g.moveToJointPosition(jts_both, pos2, max_velocity_scaling_factor = 1, plan_only=False)
            # Initialize the pickgoal.
	    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		
	    pickgoal.pose.orientation.x = 1.0
	    pickgoal.pose.orientation.y = 0.0
	    pickgoal.pose.orientation.z = 0.0
	    pickgoal.pose.orientation.w = 0.0

            # Move to the approach pickgoal. (5 cm to pickgoal)
	    pickgoal.pose.position.z = zn+0.05
	    # 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", max_velocity_scaling_factor = 1, plan_only=False)
	    pickgoal.pose.position.z = zn
            # Move to the pickgoal.
	    gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 0.3, plan_only=False)
            # Read the force in z direction.
            b=leftarm.endpoint_effort()
            z_= b['force']
	    z_force= z_.z
            # Search again for objects, if the gripper isn't at the right position and presses on an object.
	    #print("----->force in z direction:", z_force)
	    if z_force>-4:
                leftgripper.close()
                attempts=0
	        pressure_ok=1
                # If the gripper hadn't enough pressure after 2 seconds it opens and search again for objects.
	        while(leftgripper.force()<25 and pressure_ok==1):   
		    time.sleep(0.04)
		    attempts+=1
		    if(attempts>50):
                        leftgripper.open()
                        pressure_ok=0
	                print("----->pressure is to low<-----")
            else:
                print("----->gripper presses on an object<-----")

            # Move back to the approach pickgoal.
	    pickgoal.pose.position.z = zn+0.05
	    gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False)
            # Move both arms to position 1.
            g.moveToJointPosition(jts_both, pos1, max_velocity_scaling_factor = 1, plan_only=False)

	    if pressure_ok and z_force>-4:
                # Define the approach placegoal.
                # Increase the height of the tower every time by 2.75 cm.
	        placegoal.pose.position.z =-0.145+(k*0.0275)+0.08
                # Move to the approach pickgoal.
	        gl.moveToPose(placegoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False)
                # Define the placegoal.
	        placegoal.pose.position.z =-0.145+(k*0.0275)
	        gl.moveToPose(placegoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False)
	        leftgripper.open()
                k += 1
		# Move to the approach placegoal.
		placegoal.pose.position.z = -0.145+(k*0.0275)+0.08
		gl.moveToPose(placegoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False)

	    # Move left arm to start position pos1.
            gl.moveToJointPosition(jts_left, lpos1, max_velocity_scaling_factor = 1, plan_only=False)  
    
    pr.disable()
    sortby = 'cumulative'
    ps=pstats.Stats(pr).sort_stats(sortby).print_stats(0.0)
    p.clear()
Пример #20
0
class GraspingClient(GripperCommandClient):
    def __init__(self):
        # initialize GripperCommandClient
        GripperCommandClient.__init__(self)
        # initialize Moveit Scene
        self.scene = PlanningSceneInterface("base_link")
        self.scene2 = moveit_commander.PlanningSceneInterface()
        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()
        # creat target pose publisher
        self.marker_pub = rospy.Publisher("/TargetMarker",
                                          MarkerArray,
                                          queue_size=1)
        # creat basket pos publisher
        self.marker_pub2 = rospy.Publisher("/basket",
                                           MarkerArray,
                                           queue_size=1)
        # creat publisher for requesting grasp pose
        self.request_cloud_client = rospy.Publisher("/request_cloud",
                                                    Int32,
                                                    queue_size=1)
        # instantiate a RobotCommander
        self.robot = moveit_commander.RobotCommander()
        # instantiate two MoveGroupCommander
        self.group = moveit_commander.MoveGroupCommander("arm")
        # basket's parameters
        self.basket_pos = 'left'
        self.basket_found = False
        self.basket_pos_x = 0
        self.basket_pos_y = 0
        self.basket_search_t = 0

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

        #remove original objects
        self.clear_scene()
        # all the objects in scene
        self.objects = list()
        # height is used for grasping selection
        self.height = -1
        # object number
        object_num = -1

        # add support surface to scene
        for obj in find_result.support_surfaces:
            # extend surface to floor
            h = obj.primitive_poses[0].position.z
            if (h + obj.primitives[0].dimensions[2] / 2.0) > self.height:
                self.height = h + obj.primitives[0].dimensions[2] / 2.0
                self.table = obj

            obj.primitives[0].dimensions = [
                obj.primitives[0].dimensions[0] + 0.02,
                obj.primitives[0].dimensions[1] + 0.02,
                obj.primitives[0].dimensions[2] + h - 0.02
            ]
            obj.primitive_poses[0].position.z += -h / 2.0
            # add to scene
            self.scene.addSolidPrimitive(obj.name,
                                         obj.primitives[0],
                                         obj.primitive_poses[0],
                                         wait=False)
        # add objects Solid to scene
        for obj in find_result.objects:
            object_num += 1
            obj.object.name = "object%d" % object_num

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

            if obj.object.primitive_poses[0].position.z > self.height:
                self.objects.append([
                    obj,
                    numpy.array([
                        obj.object.primitive_poses[0].position.x,
                        obj.object.primitive_poses[0].position.y,
                        obj.object.primitive_poses[0].position.z
                    ]), obj.object.primitive_poses[0].position.x
                ])

            # localize basket
            if obj.object.primitive_poses[0].position.y > self.table.primitive_poses[0].position.y + self.table.primitives[0].dimensions[1] / 2.0 + 0.1 or \
             obj.object.primitive_poses[0].position.y < self.table.primitive_poses[0].position.y - self.table.primitives[0].dimensions[1] / 2.0 - 0.1:
                if obj.object.primitives[0].dimensions[
                        1] > 0.20 and obj.object.primitive_poses[
                            0].position.z > 0.3 and not self.basket_found:
                    if self.basket_search_t == 1:
                        self.basket_pos_x = obj.object.primitive_poses[
                            0].position.x
                        self.basket_pos_y = obj.object.primitive_poses[
                            0].position.y
                        self.basket_search_t += 1
                    elif self.basket_search_t == 2:
                        if self.basket_pos_x - obj.object.primitive_poses[0].position.x < 0.04 and \
                         self.basket_pos_y - obj.object.primitive_poses[0].position.y < 0.04:
                            self.basket_found = True
                            if obj.object.primitive_poses[0].position.y < 0:
                                self.basket_pos = 'right'
                            self.basket = obj
                            self.marker_pub2.publish(self.basket_marker())
                            rospy.loginfo("Basket is found at %s..." %
                                          self.basket_pos)
                        else:
                            self.basket_search_t -= 1

        if self.basket_search_t == 0:
            self.basket_search_t += 1

        self.scene.waitForSync()

    def clear_scene(self):
        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()

    def clear_target_object(self, pose):
        if len(self.objects) == 1:
            self.scene.removeCollisionObject(self.objects[0][0].object.name)
            self.tname = self.objects[0][0].object.name
        else:
            dist = float('inf')
            vec = numpy.array([
                pose[0].center.x + 0.33 * pose[0].approach.x,
                pose[0].center.y + 0.33 * pose[0].approach.y,
                pose[0].center.z + 0.33 * pose[0].approach.z
            ])
            for obj in self.objects:
                temp = numpy.linalg.norm(obj[1] - vec)
                if temp < dist:
                    dist = temp
                    self.tname = obj[0].object.name
            self.scene.removeCollisionObject(self.tname, False)

    def readd_target_object(self):
        for obj in self.objects:
            if obj[0].object.name == self.tname:
                self.scene.addSolidPrimitive(obj[0].object.name,
                                             obj[0].object.primitives[0],
                                             obj[0].object.primitive_poses[0],
                                             wait=False)

    def request_grasp_poses(self, mode=None):
        # possible grasp poses
        self.grasp_pose = list()

        rospy.wait_for_service('grasp_pose')
        res = rospy.ServiceProxy('grasp_pose', grasp_pose)

        grasps_obtained = False
        while not grasps_obtained:
            try:
                self.grasp_pose[:] = []
                rospy.loginfo("Try to find feasible grasp poses ...")
                # request PointCloud data for agile grasp package
                self.request_cloud_client.publish(1)
                grasps = res()
                if mode == 'cluster':
                    self.pose_selection_cluster(grasps.grasps)
                else:
                    self.pose_selection_general(grasps.grasps)
                if len(self.grasp_pose) > 0:
                    self.TargetGraspPoses, self.Rots = self.creat_target_grasp_pose(
                        self.grasp_pose)
                    if self.TargetGraspPoses != None:
                        grasps_obtained = True
            except rospy.ServiceException, e:
                rospy.loginfo("Service call failed: %s" % e)
                continue
Пример #21
0
# box_pose = geometry_msgs.msg.PoseStamped()
# box_pose.header.frame_id = robot.get_planning_frame()
# box_pose.pose.position.x = 1.
# box_pose.pose.position.y = 0.5
# box_pose.pose.position.z = 0.5
# scene.add_box("table", box_pose, (0.5, 1.5, 0.6))

# name size and position
p.addBox("first part", 0.8, 0.43, 0.04, 1.23, 0, 0.36)
p.addBox("under part", 0.8, 0.43, 0.04, 1.23, 0, 0.0)

p.addBox("second part", 0.8, 0.01, 1.6, 1.23, -0.22, 0)
p.addBox("third part", 0.8, 0.01, 1.6, 1.23, 0.22, 0)

p.waitForSync()

def load_gazebo_models(table_pose=Pose(position=Point(x=0.7, y=0.0, z=0.0)),
                       table_reference_frame="world",
                       block_pose1=Pose(position=Point(x=0.6725, y=0.0765, z=-0.135)),
                       block_pose2=Pose(position=Point(x=0.55, y=-0.2, z=-0.135)),
                       block_pose3=Pose(position=Point(x=0.7, y=-0.1, z=-0.135)),
                       block_pose4=Pose(position=Point(x=0.58, y=-0.03, z=-0.135)),
                       block_reference_frame="base"):
    pass
    # Get Models' Path.
    model_path = rospkg.RosPack().get_path('baxter_pnp_one_arm_cartesian_sim')+"/models/"
    # Load Table SDF.
    table_xml = ''
    with open (model_path + "cafe_table/model.sdf", "r") as table_file:
        table_xml=table_file.read().replace('\n', '')
Пример #22
0
if __name__ == "__main__":
    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()
Пример #23
0
def clasificar():

    adaptative = True
    pr_b = False
    precision = 0.7

    # Can free memory?
    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 pos
    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
    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.65
    lpos.pose.position.y = 0.6
    lpos.pose.position.z = 0.206
    lpos.pose.orientation.x = 1.0
    lpos.pose.orientation.y = 0.0
    lpos.pose.orientation.z = 0.0
    lpos.pose.orientation.w = 0.0

    while True:

        # Move to initial position
        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
        locs = PoseArray()
        locs = rospy.wait_for_message("clasificacion", PoseArray)

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

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

            alfa = 0.1

            # Two parameters:
            # When pr_b == 1, the program will try to adapt the trajectory for getting the precision established in the parameter of the same name
            # When adaptative == 1, the program will try to get the best precision based on the precision of the last execution.

            if (pr_b and not adaptative):
                if precision >= 1:
                    punto_medio.pose.position.x += 0.01

                else:
                    punto_medio.pose.position.x -= alfa * (1 - precision)

            else:

                print("If it is the first time executing, write -1")
                precision_value = input()

                if precision_value != -1.0:
                    punto_medio.pose.position.x += alfa * (1 - precision_value)
                elif precision_value == 1.0:
                    adaptative = False

        # Get the normal orientation for separating the objects
            orient = (-1) * (1 / punto_medio.pose.orientation.x)

            punto_medio.pose = rotate_pose_msg_by_euler_angles(
                punto_medio.pose, 0.0, 0.0, orient)
            rightarm_group.moveToPose(punto_medio,
                                      "right_gripper",
                                      max_velocity_scaling_factor=1,
                                      plan_only=False)

            orient = 1.57 - orient
            punto_medio.pose = rotate_pose_msg_by_euler_angles(
                punto_medio.pose, 0.0, 0.0, orient)
            rightarm_group.moveToPose(punto_medio,
                                      "right_gripper",
                                      max_velocity_scaling_factor=1,
                                      plan_only=False)

            punto_medio.pose.position.x += 0.15
            rightarm_group.moveToPose(punto_medio,
                                      "right_gripper",
                                      max_velocity_scaling_factor=1,
                                      plan_only=False)
            time.sleep(1)
            punto_medio.pose.position.x -= 0.3
            rightarm_group.moveToPose(punto_medio,
                                      "right_gripper",
                                      max_velocity_scaling_factor=1,
                                      plan_only=False)
            time.sleep(1)

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

            # Free the memory when finished
            freemem = "free"
            if not adaptative:
                pub.publish(freemem)

        else:
            sys.exit("Cannot identify any object")
Пример #24
0
class GraspingClient(object):
    def __init__(self, sim=False):
        self.scene = PlanningSceneInterface("base_link")
        self.dof = rospy.get_param('~jaco_dof')
        self.move_group = MoveGroupInterface("upper_body", "base_link")
        self.lmove_group = MoveGroupInterface("left_arm", "base_link")
        self.rmove_group = MoveGroupInterface("right_arm", "base_link")
        self.move_group.setPlannerId("RRTConnectkConfigDefault")
        self.lmove_group.setPlannerId("RRTConnectkConfigDefault")
        self.rmove_group.setPlannerId("RRTConnectkConfigDefault")

        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.17, -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.larm_const_stow = [-2.28, -2.17, 2.56, 0.09, -0.15, 2.08]
            self.rarm_const_stow = [2.28, 2.17, -2.56, -0.09, 0.15, 1.06]
            self.tableDist = 0.7

        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.larm_const_stow = [2.6, -2.0, 0.0, -2.0, 0.0, 0.0, 1.0]
            self.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0]
            self.tableDist = 0.8

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

        self.pickplace = [None] * 2
        self.pickplace[0] = PickPlaceInterface("left_side",
                                               "left_gripper",
                                               verbose=True)
        self.pickplace[0].planner_id = "RRTConnectkConfigDefault"
        self.pickplace[1] = PickPlaceInterface("right_side",
                                               "right_gripper",
                                               verbose=True)
        self.pickplace[1].planner_id = "RRTConnectkConfigDefault"
        self.pick_result = [None] * 2
        self._last_gripper_picked = 0
        self.place_result = [None] * 2
        self._last_gripper_placed = 0

        self._objs_to_keep = []

        self._listener = tf.TransformListener()

        self._lgripper = GripperActionClient('left')
        self._rgripper = GripperActionClient('right')

        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.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 add_objects_to_keep(self, obj):
        self._objs_to_keep.append(obj)

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

    def getPickCoordinates(self):

        self.updateScene(0, False)
        beer, grasps = self.getGraspableBeer(False)
        pringles, grasps = self.getGraspablePringles(False)
        if (None == beer) or (None == pringles):
            return None
        center_objects = (beer.primitive_poses[0].position.y +
                          pringles.primitive_poses[0].position.y) / 2

        surface = self.getSupportSurface(beer.support_surface)
        tmp1 = surface.primitive_poses[
            0].position.x - surface.primitives[0].dimensions[0] / 2
        surface = self.getSupportSurface(pringles.support_surface)
        tmp2 = surface.primitive_poses[
            0].position.x - surface.primitives[0].dimensions[0] / 2
        front_edge = (tmp1 + tmp2) / 2

        coords = Pose2D(x=(front_edge - self.tableDist),
                        y=center_objects,
                        theta=0.0)

        return coords

    def updateScene(self, gripper=0, plan=True):
        # find objects
        rospy.loginfo("Updating scene...")
        goal = FindGraspableObjectsGoal()
        goal.plan_grasps = plan
        goal.gripper = gripper
        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, True)

        # insert objects to scene
        idx = -1
        for obj in find_result.objects:
            if obj.object.primitive_poses[
                    0].position.z < 0.5 or obj.object.primitive_poses[
                        0].position.x > 2.0 or obj.object.primitive_poses[
                            0].position.y > 0.5:
                continue
            idx += 1
            obj.object.name = "object%d_%d" % (idx, gripper)
            self.scene.addSolidPrimitive(obj.object.name,
                                         obj.object.primitives[0],
                                         obj.object.primitive_poses[0],
                                         wait=True)

        for obj in find_result.support_surfaces:
            # extend surface to floor, and make wider since we have narrow field of view
            if obj.primitive_poses[0].position.z < 0.5:
                continue
            height = obj.primitive_poses[0].position.z
            obj.primitives[0].dimensions = [
                obj.primitives[0].dimensions[0],
                obj.primitives[0].dimensions[1],  # 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=True)

        self.scene.waitForSync()

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

    def getGraspableBeer(self, planned=True):
        for obj in self.objects:

            # need grasps
            if len(obj.grasps) < 1 and planned:
                continue

            # has to be on table
            if obj.object.primitive_poses[0].position.z < 0.5 or \
               obj.object.primitive_poses[0].position.z > 1.0 or \
               obj.object.primitive_poses[0].position.x > 2.0:
                continue
            elif (obj.object.primitives[0].type == sp.CYLINDER):
                if obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] < 0.102 or \
                   obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] > 0.142:
                    continue
            elif (obj.object.primitives[0].type == sp.BOX):
                if obj.object.primitives[0].dimensions[sp.BOX_Z] < 0.102 or \
                   obj.object.primitives[0].dimensions[sp.BOX_Z] > 0.142:
                    continue
            else:
                continue

            print "beer:   ", obj.object.primitive_poses[0]

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

    def getGraspablePringles(self, planned=True):
        for obj in self.objects:
            # need grasps
            if len(obj.grasps) < 1 and planned:
                continue

            # has to be on table
            if obj.object.primitive_poses[0].position.z < 0.5 or \
               obj.object.primitive_poses[0].position.z > 1.0 or \
               obj.object.primitive_poses[0].position.x > 2.0:
                continue
            elif (obj.object.primitives[0].type == sp.CYLINDER):
                if obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] < 0.21 or \
                   obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] > 0.28:
                    continue
            elif (obj.object.primitives[0].type == sp.BOX):
                if obj.object.primitives[0].dimensions[sp.BOX_Z] < 0.21 or \
                   obj.object.primitives[0].dimensions[sp.BOX_Z] > 0.28:
                    continue
            else:
                continue

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

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

    def getPlaceLocation(self):
        pass

    def pick(self, block, grasps, gripper=0):

        success, pick_result = self.pickplace[gripper].pick_with_retry(
            block.name,
            grasps,
            retries=1,
            support_name=block.support_surface,
            scene=self.scene)
        self.pick_result[gripper] = pick_result
        self._last_gripper_picked = gripper
        return success

    def place(self, block, pose_stamped, gripper=0):
        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[
            gripper].grasp.pre_grasp_posture
        l.pre_place_approach = self.pick_result[
            gripper].grasp.pre_grasp_approach
        l.post_place_retreat = self.pick_result[
            gripper].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[gripper].place_with_retry(
            block.name, places, retries=1, scene=self.scene)
        self.place_result[gripper] = place_result
        self._last_gripper_placed = gripper
        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,
                path_constraints=c1,
                planning_time=120.0)
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                return

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

    def close_gripper(self):
        self._lgripper.command(self._gripper_closed, block=True)
        self._rgripper.command(self._gripper_closed, block=True)
Пример #25
0
        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()

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 += .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)
Пример #27
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))
Пример #28
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()
        """
        might be useful if we want to pick and place multiple 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
        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
        """
        can change the place location based on the pick location. Let the pick location be the origin.
        """
        # 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
Пример #29
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
Пример #30
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")

        # 初始化需要使用move group控制的机械臂中的arm group
        self.arm = MoveGroupCommander('arm')

        # 设置位置(单位:米)和姿态(单位:弧度)的允许误差
        self.arm.set_goal_position_tolerance(0.01)
        self.arm.set_goal_orientation_tolerance(0.05)

        # 当运动规划失败后,允许重新规划
        self.arm.allow_replanning(True)

        # 设置目标位置所使用的参考坐标系
        reference_frame = 'base_link'
        self.arm.set_pose_reference_frame(reference_frame)

        # 设置每次运动规划的时间限制:5s
        self.arm.set_planning_time(5)
        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
        objects = list()
        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)
            if obj.object.primitive_poses[0].position.x < 0.85:
                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
            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

        # store graspable objects by Z
        objects.sort(key=lambda object: object[1])
        objects.reverse()
        self.objects = [object[0] for object in 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:
                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.5:
                continue
            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 check_collision(self, pose): # [0, 0, 0, 0, 0, 0, 0]
        self.arm.set_joint_value_target(pose)
        # 控制机械臂完成运动
        #print self.arm.go()
        #self.arm.set_joint_value_target([1.5, -0.6, 3.0, 1.0, 3.0, 1.0, 3.0])
        return self.arm.go()

    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
Пример #31
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)
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)
Пример #33
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)
class MoveItDemo:
    def __init__(self):
        rospy.init_node('moveit_demo')

        # We need a tf2 listener to convert poses into arm reference base
        try:
            self._tf2_buff = tf2_ros.Buffer()
            self._tf2_list = tf2_ros.TransformListener(self._tf2_buff)
        except rospy.ROSException as err:
            rospy.logerr("MoveItDemo: could not start tf buffer client: " + str(err))
            raise err

        self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening") - 0.01]
        self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening") + 0.01]
        self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral",
                                                (self.gripper_opened[0] + self.gripper_closed[0])/2.0) ]
        
        self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten", 0.0) 

        # Use the planning scene object to add or remove objects
        self.scene = PlanningSceneInterface(REFERENCE_FRAME)

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

        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('target_pose', PoseStamped, queue_size=10)

        # Create a dictionary to hold object colors
        self.colors = dict()

        self.server = InteractiveMarkerServer("obj_find")

        # Initialize the move group for the right arm
        #######arm = MoveGroupCommander(GROUP_NAME_ARM)
        self.arm = MoveGroupCommander(GROUP_NAME_ARM)

        # Initialize the move group for the right gripper
        #######gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
        self.gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)

        # Get the name of the end-effector link
        #######end_effector_link = arm.get_end_effector_link()
        self.end_effector_link = self.arm.get_end_effector_link()

        # Allow some leeway in position (meters) and orientation (radians)
        #######arm.set_goal_position_tolerance(0.05)
        #######arm.set_goal_orientation_tolerance(0.1)
        self.arm.set_goal_position_tolerance(0.05)
        self.arm.set_goal_orientation_tolerance(0.1)

        # Allow replanning to increase the odds of a solution
        #######arm.allow_replanning(True)
        self.arm.allow_replanning(True)

        # Set the right arm reference frame
        #######arm.set_pose_reference_frame(REFERENCE_FRAME)
        self.arm.set_pose_reference_frame(REFERENCE_FRAME)

        # Allow 5 seconds per planning attempt
        #######arm.set_planning_time(15)
        self.arm.set_planning_time(15)

        # Set a limit on the number of pick attempts before bailing
        #######max_pick_attempts = 1
        self.max_pick_attempts = 1

        # Set a limit on the number of place attempts
        ########max_place_attempts = 3
        self.max_place_attempts = 3

        # Give the scene a chance to catch up
        rospy.sleep(2)
        '''
        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")

        rospy.sleep(1)
        '''
        self.arm.set_named_target('right_up')
        if self.arm.go() != True:
            rospy.logwarn("  Go failed")

        self.gripper.set_joint_value_target(self.gripper_opened)
        if self.gripper.go() != True:
            rospy.logwarn("  Go failed")

        rospy.sleep(1)

        
        # Initialize the grasping goal
        #goal = FindGraspableObjectsGoal()

        #goal.plan_grasps = False
        '''
        find_objects.send_goal(goal)

        find_objects.wait_for_result(rospy.Duration(5.0))

        find_result = find_objects.get_result()

        rospy.loginfo("Found %d objects" %len(find_result.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.scene._colors = dict()

        # Use the nearest object on the table as the target
        self.target_pose = PoseStamped()
        self.target_pose.header.frame_id = REFERENCE_FRAME
        self.target_id = 'target'
        self.target_size = None
        self.the_object = None
        self.the_object_dist = 0.30
        self.the_object_dist_min = 0.10
        self.count = -1
        
        #for obj in find_result.objects:
        #    count +=1
            
        #dx = obj.object.primitive_poses[0].position.x
        #dy = obj.object.primitive_poses[0].position.y
        dx = 0.25
        dy = 0.0
        d = math.sqrt((dx * dx) + (dy * dy))
        m = Marker()
        m.type = Marker.CUBE
        m.scale.x = 0.02
        m.scale.y = 0.02
        m.scale.z = 0.05
        m.color.r = 0.0
        m.color.g = 0.5
        m.color.b = 0.5
        m.color.a = 1.0

        int_marker = InteractiveMarker()
        int_marker.header.frame_id = "base_link"
        int_marker.name = "block_1"
        int_marker.pose.position.x = dx
        int_marker.pose.position.y = dy
        int_marker.pose.position.z = 0.04

        int_marker.pose.orientation.x = 0.0 
        int_marker.pose.orientation.y = 0.0
        int_marker.pose.orientation.z = 0.0
        int_marker.pose.orientation.w = 1.0

        control =  InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 0
        control.orientation.y = 1
        control.orientation.z = 0
        control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
        
        int_marker.controls.append(control)
        control.markers.append(m)
        control.always_visible = True
        int_marker.controls.append(control)

        self.server.insert(int_marker,self.processFeedback)
        self.server.applyChanges()

    def processFeedback(self,feedback):
        #p.pose.position = feedback.pose.position
        #self.dist_xy.pose.position = feedback.pose.position
        old_pose = Pose()
        new_pose = Pose()
        result = None
        
        if feedback.event_type == visualization_msgs.msg.InteractiveMarkerFeedback.MOUSE_DOWN:
            old_pose.position.x = feedback.pose.position.x 
            old_pose.position.y = feedback.pose.position.y
            result = self.pick_pose(old_pose)
            print "old pose is " + str(old_pose.position.x) + " , " + str(old_pose.position.y)

        if feedback.event_type == visualization_msgs.msg.InteractiveMarkerFeedback.MOUSE_UP:
            new_pose.position.x = feedback.pose.position.x
            new_pose.position.y = feedback.pose.position.y 
            self.pick_and_place_pose(old_pose, new_pose)
            print "new pose is " + str(new_pose.position.x) + " , " + str(new_pose.position.y)

    def pick_and_place_pose(self,pick_pose,place_pose):
        pass

    def pick_pose(self, pick_pose):
        moveit_commander.roscpp_initialize(sys.argv)
        dx = pick_pose.position.x
        dy = pick_pose.position.y
        d = math.sqrt((dx * dx) + (dy * dy))
        
        if d < self.the_object_dist and d > self.the_object_dist_min:
            #if dx > the_object_dist_xmin and dx < the_object_dist_xmax:
            #    if dy > the_object_dist_ymin and dy < the_object_dist_ymax:
            rospy.loginfo("object is in the working zone")
            self.the_object_dist = d

            self.the_object = self.count

            self.target_size = [0.02, 0.02, 0.05]

            #target_pose.pose.position.x = obj.object.primitive_poses[0].position.x + target_size[0] / 2.0
            #target_pose.pose.position.y = obj.object.primitive_poses[0].position.y 
            self.target_pose.pose.position.x = dx + self.target_size[0] / 2.0
            self.target_pose.pose.position.y = dy 
            self.target_pose.pose.position.z = 0.04

            self.target_pose.pose.orientation.x = 0.0
            self.target_pose.pose.orientation.y = 0.0
            self.target_pose.pose.orientation.z = 0.0
            self.target_pose.pose.orientation.w = 1.0

            # wait for the scene to sync
            self.scene.waitForSync()
            self.scene.setColor(self.target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0)
            self.scene.sendColors()

            grasp_pose = self.target_pose

            grasp_pose.pose.position.y -= self.target_size[1] / 2.0
            grasp_pose.pose.position.x += self.target_size[0]

            grasps = self.make_grasps(grasp_pose, [self.target_id], [self.target_size[1] - self.gripper_tighten])

            # Track success/failure and number of attempts for pick operation
            '''
            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
            self.arm.set_start_state_to_current_state()
                
            # Repeat until we succeed or run out of attempts
            while result != MoveItErrorCodes.SUCCESS and n_attempts < self.max_pick_attempts:
                result = self.arm.pick(self.target_id, grasps)
                n_attempts += 1
                rospy.loginfo("Pick attempt: " +  str(n_attempts))
                rospy.sleep(1.0)
                result = self.arm.pick(self.target_id, grasps)
        else:
            rospy.loginfo("object is not in the working zone")
            rospy.sleep(1)

        self.arm.set_named_target('right_up')
        self.arm.go()
    
        # Open the gripper to the neutral position
        self.gripper.set_joint_value_target(self.gripper_neutral)
        self.gripper.go()

        rospy.sleep(1)
        return result

    # Get the gripper posture as a JointTrajectory
    def make_gripper_posture(self, joint_positions):
        # Initialize the joint trajectory for the gripper joints
        t = JointTrajectory()

        # Set the joint names to the gripper joint names
        t.header.stamp = rospy.get_rostime()
        t.joint_names = GRIPPER_JOINT_NAMES

        # Initialize a joint trajectory point to represent the goal
        tp = JointTrajectoryPoint()

        # Assign the trajectory joint positions to the input positions
        tp.positions = joint_positions

        # Set the gripper effort
        tp.effort = GRIPPER_EFFORT

        tp.time_from_start = rospy.Duration(0.0)

        # Append the goal point to the trajectory points
        t.points.append(tp)

        # Return the joint trajectory
        return t

    # Generate a gripper translation in the direction given by vector
    def make_gripper_translation(self, min_dist, desired, vector):
        # Initialize the gripper translation object
        g = GripperTranslation()

        # Set the direction vector components to the input
        g.direction.vector.x = vector[0]
        g.direction.vector.y = vector[1]
        g.direction.vector.z = vector[2]

        # The vector is relative to the gripper frame
        g.direction.header.frame_id = GRIPPER_FRAME

        # Assign the min and desired distances from the input
        g.min_distance = min_dist
        g.desired_distance = desired

        return g

    # Generate a list of possible grasps
    def make_grasps(self, initial_pose_stamped, allowed_touch_objects, grasp_opening=[0]):
        # Initialize the grasp object
        g = Grasp()

        # Set the pre-grasp and grasp postures appropriately;
        # grasp_opening should be a bit smaller than target width
        g.pre_grasp_posture = self.make_gripper_posture(self.gripper_opened)
        g.grasp_posture = self.make_gripper_posture(grasp_opening)

        # Set the approach and retreat parameters as desired
        g.pre_grasp_approach = self.make_gripper_translation(0.1, 0.15, [0.0, 0.0, -1.5])
        g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.15, [0.0, 0.0, 1.5])

        # Set the first grasp pose to the input pose
        g.grasp_pose = initial_pose_stamped

        # Pitch angles to try
        pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4, 0.5, -0.5, 0.6, -0.6, 0.7, -0.7, 0.8, -0.8, 0.9, -0.9, 1.0, -1.0, 1.1, -1.1, 1.2, -1.2, 1.3, -1.3, 1.4, -1.4, 1.5, -1.5]

        # Yaw angles to try; given the limited dofs of turtlebot_arm, we must calculate the heading
        # from arm base to the object to pick (first we must transform its pose to arm base frame)
        target_pose_arm_ref = self._tf2_buff.transform(initial_pose_stamped, ARM_BASE_FRAME)
        x = target_pose_arm_ref.pose.position.x
        y = target_pose_arm_ref.pose.position.y
        yaw_vals = [atan2(y, x) + inc for inc in [0, 0.1,-0.1]]

        # A list to hold the grasps
        grasps = []

        # Generate a grasp for each pitch and yaw angle
        for yaw in yaw_vals:
            for pitch in pitch_vals:
                # Create a quaternion from the Euler angles
                q = quaternion_from_euler(0, pitch, yaw)

                # Set the grasp pose orientation accordingly
                g.grasp_pose.pose.orientation.x = q[0]
                g.grasp_pose.pose.orientation.y = q[1]
                g.grasp_pose.pose.orientation.z = q[2]
                g.grasp_pose.pose.orientation.w = q[3]

                # Set and id for this grasp (simply needs to be unique)
                g.id = str(len(grasps))

                # Set the allowed touch objects to the input list
                g.allowed_touch_objects = allowed_touch_objects

                # Don't restrict contact force
                g.max_contact_force = 0

                # Degrade grasp quality for increasing pitch angles
                g.grasp_quality = 1.0 - abs(pitch)

                # Append the grasp to the list
                grasps.append(deepcopy(g))

        # Return the list
        return grasps

    # Generate a list of possible place poses
    def make_places(self, target_id, init_pose):
        # Initialize the place location as a PoseStamped message
        place = PoseStamped()

        # Start with the input place pose
        place = init_pose

        # A list of x shifts (meters) to try
        x_vals = [0, 0.005, -0.005] #, 0.01, -0.01, 0.015, -0.015]

        # A list of y shifts (meters) to try
        y_vals = [0, 0.005, -0.005, 0.01, -0.01] #, 0.015, -0.015]

        # A list of pitch angles to try
        pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4, 0.5, -0.5, 0.6, -0.6, 0.7, -0.7, 0.8, -0.8, 0.9, -0.9, 1.0, -1.0, 1.1, -1.1, 1.2, -1.2, 1.3, -1.3, 1.4, -1.4, 1.5, -1.5] #, 0.005, -0.005, 0.01, -0.01, 0.02, -0.02]

        # A list to hold the places
        places = []

        # Generate a place pose for each angle and translation
        for pitch in pitch_vals:
            for dy in y_vals:
                for dx in x_vals:
                    place.pose.position.x = init_pose.pose.position.x + dx
                    place.pose.position.y = init_pose.pose.position.y + dy
    
                    # Yaw angle: given the limited dofs of turtlebot_arm, we must calculate the heading from
                    # arm base to the place location (first we must transform its pose to arm base frame)
                    target_pose_arm_ref = self._tf2_buff.transform(place, ARM_BASE_FRAME)
                    x = target_pose_arm_ref.pose.position.x
                    y = target_pose_arm_ref.pose.position.y
                    yaw = atan2(y, x)
    
                    # Create a quaternion from the Euler angles
                    q = quaternion_from_euler(0, pitch, yaw)
    
                    # Set the place pose orientation accordingly
                    place.pose.orientation.x = q[0]
                    place.pose.orientation.y = q[1]
                    place.pose.orientation.z = q[2]
                    place.pose.orientation.w = q[3]

                    # MoveGroup::place will transform the provided place pose with the attached body pose, so the object retains
                    # the orientation it had when picked. However, with our 4-dofs arm this is infeasible (nor we care about the
                    # objects orientation!), so we cancel this transformation. It is applied here:
                    # https://github.com/ros-planning/moveit_ros/blob/jade-devel/manipulation/pick_place/src/place.cpp#L64
                    # More details on this issue: https://github.com/ros-planning/moveit_ros/issues/577
                    acobjs = self.scene.get_attached_objects([target_id])
                    aco_pose = self.get_pose(acobjs[target_id])
                    if aco_pose is None:
                        rospy.logerr("Attached collision object '%s' not found" % target_id)
                        return None

                    aco_tf = self.pose_to_mat(aco_pose)
                    place_tf = self.pose_to_mat(place.pose)
                    place.pose = self.mat_to_pose(place_tf, aco_tf)
                    rospy.logdebug("Compensate place pose with the attached object pose [%s]. Results: [%s]" \
                                   % (aco_pose, place.pose))

                    # Append this place pose to the list
                    places.append(deepcopy(place))

        # Return the list
        return places

    # Set the color of an object
    def setColor(self, name, r, g, b, a=0.9):
        # Initialize a MoveIt color object
        color = ObjectColor()

        # Set the id to the name given as an argument
        color.id = name

        # Set the rgb and alpha values given as input
        color.color.r = r
        color.color.g = g
        color.color.b = b
        color.color.a = a

        # Update the global color dictionary
        self.colors[name] = color

    # Actually send the colors to MoveIt!
    def sendColors(self):
        # Initialize a planning scene object
        p = PlanningScene()

        # Need to publish a planning scene diff
        p.is_diff = True

        # Append the colors from the global color dictionary
        for color in self.colors.values():
            p.object_colors.append(color)

        # Publish the scene diff
        self.scene_pub.publish(p)

    def get_pose(self, co):
        # We get object's pose from the mesh/primitive poses; try first with the meshes
        if isinstance(co, CollisionObject):
            if co.mesh_poses:
                return co.mesh_poses[0]
            elif co.primitive_poses:
                return co.primitive_poses[0]
            else:
                rospy.logerr("Collision object '%s' has no mesh/primitive poses" % co.id)
                return None
        elif isinstance(co, AttachedCollisionObject):
            if co.object.mesh_poses:
                return co.object.mesh_poses[0]
            elif co.object.primitive_poses:
                return co.object.primitive_poses[0]
            else:
                rospy.logerr("Attached collision object '%s' has no mesh/primitive poses" % co.id)
                return None
        else:
            rospy.logerr("Input parameter is not a collision object")
            return None

    def pose_to_mat(self, pose):
        '''Convert a pose message to a 4x4 numpy matrix.

        Args:
            pose (geometry_msgs.msg.Pose): Pose rospy message class.

        Returns:
            mat (numpy.matrix): 4x4 numpy matrix
        '''
        quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
        pos = np.matrix([pose.position.x, pose.position.y, pose.position.z]).T
        mat = np.matrix(quaternion_matrix(quat))
        mat[0:3, 3] = pos
        return mat

    def mat_to_pose(self, mat, transform=None):
        '''Convert a homogeneous matrix to a Pose message, optionally premultiply by a transform.

        Args:
            mat (numpy.ndarray): 4x4 array (or matrix) representing a homogenous transform.
            transform (numpy.ndarray): Optional 4x4 array representing additional transform

        Returns:
            pose (geometry_msgs.msg.Pose): Pose message representing transform.
        '''
        if transform != None:
            mat = np.dot(mat, transform)
        pose = Pose()
        pose.position.x = mat[0,3]
        pose.position.y = mat[1,3]
        pose.position.z = mat[2,3]
        quat = quaternion_from_matrix(mat)
        pose.orientation.x = quat[0]
        pose.orientation.y = quat[1]
        pose.orientation.z = quat[2]
        pose.orientation.w = quat[3]
        return pose
Пример #35
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
Пример #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()

        # #######################################################################
        # # test fetch_grasp_planner_node/plan with pose subcriber
        # # use fetch_grasp_planner_node/plan with fetch_grasp_planner_node/plan
        # grasp_topic = "fetch_grasp_planner_node/plan"
        # rospy.loginfo("Waiting for %s..." % grasp_topic)
        # self.grasp_planner_client = actionlib.SimpleActionClient(grasp_topic, GraspPlanningAction)
        # self.grasp_planner_client.wait_for_server()
        # #######################################################################

    #     #####################################################################
    #     # test fetch_grasp_planner_node/plan with pose subcriber
    #     rospy.Subscriber("perception/apple_pose", Pose, self.apple_pose_callback)
    #     self.object = Object()
    #     self.grasps = Grasp()

    # def apple_pose_callback(self, message):
    #     apple = SolidPrimitive()
    #     apple.type = SolidPrimitive.SPHERE
    #     apple.dimensions = 0.04
    #     self.object.primitives = apple
    #     self.object.primitive_poses = message
    #     # add stamp and frame
    #     self.object.header.stamp = rospy.Time.now()
    #     self.object.header.frame_id = message.frame_id

    #     goal = GraspPlanningGoal()
    #     goal.object = self.object
    #     self.grasp_planner_client.send_goal(goal)
    #     self.grasp_planner_client.wait_for_result(rospy.Duration(5.0))
    #     grasp_planner_result = self.grasp_planner_client.get_result()
    #     self.grasps = grasp_planner_result.grasps
    #     ####################################################################

    def updateScene(self):
        # ####################################################################
        # # use fetch_grasp_planner_node/plan with fetch_grasp_planner_node/plan
        # # find objects
        # goal = FindGraspableObjectsGoal()
        # goal.plan_grasps = False
        # self.find_client.send_goal(goal)
        # self.find_client.wait_for_result(rospy.Duration(5.0))
        # find_result = self.find_client.get_result()

        # for obj in find_result.objects:
        #     goal = GraspPlanningGoal()
        #     goal.object = obj.object
        #     self.grasp_planner_client.send_goal(goal)
        #     self.grasp_planner_client.wait_for_result(rospy.Duration(5.0))
        #     grasp_planner_result = self.grasp_planner_client.get_result()
        #     obj.grasps = grasp_planner_result.grasps
        # ####################################################################

        # 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
        objects = list()
        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)
            if obj.object.primitive_poses[0].position.x < 0.85:
                objects.append([obj, obj.object.primitive_poses[0].position.z])
            print("object ", idx, " frame_id = ", obj.object.header.frame_id)
            print("object ", idx, " pose = ", 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],
                                         use_service=False)

        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]
        #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:
                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.5:
                continue
            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,
                                                              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