Exemple #1
0
def main():
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
    required = parser.add_argument_group('required arguments')
    required.add_argument(
        '-l', '--limb', required=True, choices=['left', 'right'],
        help='send joint trajectory to which limb'
    )

    args = parser.parse_args(rospy.myargv()[1:])
    limb = args.limb

    rospy.init_node('stackit')

    iksvc, ns = ik_command.connect_service(limb)
    moveit_commander.roscpp_initialize(sys.argv)

    rate = rospy.Rate(1)
    
    robot = moveit_commander.RobotCommander()
    scene = moveit_commander.PlanningSceneInterface()
    group = moveit_commander.MoveGroupCommander(limb+"_arm")
    group.allow_replanning(True)

    print("Getting robot state... ")
    rs = baxter_interface.RobotEnable(CHECK_VERSION)
    print("Enabling robot... ")
    rs.enable()

    #Calibrate gripper
    gripper_if = baxter_interface.Gripper(limb)
    if not gripper_if.calibrated():
        print "Calibrating gripper"
        gripper_if.calibrate()

    obj_manager = ObjectManager()
    while len(obj_manager.collision_objects) <= 0:
        rate.sleep()
    objects = obj_manager.collision_objects

    object_height = params['object_height']
    
    """if len(objects) > 1:
        stack_pose = projectPose(objects[len(objects)-1].primitive_poses[0])
        stack_pose = incrementPoseMsgZ(stack_pose, object_height*2.0)
        objects.pop(len(objects)-1)
    elif len(objects) == 1:
        stack_pose = projectPose(objects[0].primitive_poses[0])"""
    stack_pose = Pose(position=Point(0.593, -0.212, object_height-0.130),
                      orientation=Quaternion(0.6509160466, 0.758886809948,
                                 -0.0180992582839, -0.0084573527776) )

    processed_win = "Processed image"
    raw_win = "Hand camera"
    cv2.namedWindow(processed_win)
    #cv2.namedWindow(raw_win)

    rospy.on_shutdown(obj_manager.remove_known_objects)

    for obj in objects:
        obj_pose = obj.primitive_poses[0]

        #group.pick(obj.id)
        #group.place(obj.id, stack_pose)

        print "Got pose:", obj.primitive_poses[0]
        pose = projectPose(obj.primitive_poses[0])
        pose = incrementPoseMsgZ(pose, object_height*1.7)
        print "Modified pose:", pose
        #if obj.id in obj_manager.id_operations:
        #    obj_manager.id_operations[obj.id] = CollisionObject.REMOVE 
        #obj_manager.publish(obj)

        print "setting target to pose"
        # Move to the next block
        group.clear_pose_targets()
        group.set_start_state_to_current_state()
        group.set_pose_target(pose)

        plan = group.plan()

        # is there a better way of checking this?
        plan_found = len(plan.joint_trajectory.points) > 0

        if plan_found:
            #print "============ Waiting while RVIZ displays plan1..."
            #rospy.sleep(3)
            group.go(wait=True)

            imgproc = ObjectFinder("star", None, None)
            imgproc.subscribe("/cameras/"+limb+"_hand_camera/image")
            imgproc.publish(limb)

            vc = VisualCommand(iksvc, limb)
            vc.subscribe()

            while (not vc.done) and (not rospy.is_shutdown()):
                blobArray = []
                
                for centroid, axis in zip(imgproc.centroids, imgproc.axes):
                    blob = BlobInfo()
                    centroid = Point(*centroid)
                    blob.centroid = centroid
                    if axis is None:
                        axis = -1*numpy.ones(6)
                    blob.axis = Polygon([Point(*axis[0:3].tolist()),
                                         Point(*axis[3:6].tolist())])
                    blobArray.append(blob)

                msg = BlobInfoArray()
                msg.blobs = blobArray
                imgproc.handler_pub.publish(msg)
                if imgproc.processed is not None:
                    cv2.imshow(processed_win, imgproc.processed)
                cv2.waitKey(10)
            vc.unsubscribe()
            imgproc.unsubscribe()

            print "Adding attached message"
            #Add attached message
            #obj.primitive_poses[0] = incrementPoseMsgZ(obj.primitive_poses[0], *object_height) #this is in the base frame...?
            obj_manager.publish_attached(obj, limb)
            #touch_links = [limb+"_gripper", limb+"_gripper_base", limb+"_hand_camera", limb+"_hand_range"]
            #group.attach_object(obj.id, limb+"_gripper", touch_links = touch_links)
            # Carefully rise away from the object before we plan another path
            pose = incrementPoseMsgZ(pose, 2*object_height) # test this
            ik_command.service_request_pose(iksvc, pose, limb, blocking = True)
            
        else:
            print "Unable to plan path"
            continue
            # what to do?

        # Move to the stacking position
        group.clear_pose_targets()
        group.set_start_state_to_current_state()
        group.set_pose_target(stack_pose)
        plan = group.plan()
        plan_found = len(plan.joint_trajectory.points) > 0

        if plan_found:
            #print "============ Waiting while RVIZ displays plan2..."
            #rospy.sleep(3)
            group.go(wait=True)
            gripper_if.open(block=True)
            group.detach_object(obj.id)
            # Carefully rise away from the object before we plan another path
            pose = incrementPoseMsgZ(stack_pose, 2*object_height)
            ik_command.service_request_pose(iksvc, pose, limb, blocking = True)
            
        
        obj.operation = CollisionObject.REMOVE
        obj_manager.publish(obj)
        # Get the next stack pose
        stack_pose = incrementPoseMsgZ(stack_pose, object_height*3/4)

        """if obj.id in obj_manager.id_operations:
Exemple #2
0
def main():
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
    required = parser.add_argument_group('required arguments')
    required.add_argument(
        '-l', '--limb', required=True, choices=['left', 'right'],
        help='send joint trajectory to which limb'
    )
    required.add_argument(
        '-f', '--folder', help='path to assets/ folder containing help images'
    )
    args = parser.parse_args(rospy.myargv()[1:])
    print args
    limb = args.limb

    print("Initializing node... ")
    rospy.init_node("servo_to_object_%s" % (limb,))
    print("Getting robot state... ")
    rs = baxter_interface.RobotEnable(CHECK_VERSION)
    print("Enabling robot... ")
    rs.enable()

    #Calibrate gripper
    gripper_if = baxter_interface.Gripper(limb)
    if not gripper_if.calibrated():
        print "Calibrating gripper"
        gripper_if.calibrate()

    if args.folder is None:
        args.folder = "/home/jackie/ros_ws/src/baxter_demos/assets/"
    # First, get start and end configurations from the user
    filenames = ["getpoint1.png", "getpoint2.png", "executing_grasp.png"]
    filenames = [args.folder+filename for filename in filenames]
    
    for filename in filenames:
        if not os.access(filename, os.R_OK):
            rospy.logerr("Cannot read file at '%s'" % (filename,))
            return 1

    limbInterface = baxter_interface.Limb(limb)

    #rospy.on_shutdown(display)

    common.send_image(filenames[0])
    #Get the current position
    buttonpress = common.ButtonListener()
    buttonpress.subscribe("/robot/digital_io/"+limb+"_lower_button/state")
    points = []

    while not buttonpress.pressed:
        rospy.sleep(params['button_rate'])
    
    points.append( buttonpress.getButtonPress(limbInterface))
    print "Got first position:"
    print points[0]

    buttonpress.pressed = False

    common.send_image(filenames[1])

    while not buttonpress.pressed:
        rospy.sleep(params['button_rate'])
 
    points.append( buttonpress.getButtonPress(limbInterface))
    print "Got second position:"
    print points[1]

    common.send_image(filenames[2])

    # Command start configuration

    limbInterface.move_to_joint_positions(points[0])

    iksvc, ns = ik_command.connect_service(limb)

    rate = rospy.Rate(params['rate'])
    dc = DepthCaller(limb, iksvc)

    # Subscribe to estimate_depth
    # Move to pose published by estimate_depth
    while (not dc.done) and (not rospy.is_shutdown()):
        rate.sleep()

    print "Starting visual servoing"
    
    # Subscribe to object_finder and start visual servoing/grasping
    vc = VisualCommand(iksvc, limb)
    vc.subscribe()

    while (not vc.done) and (not rospy.is_shutdown()):
        rate.sleep()

    limbInterface.move_to_joint_positions(points[1])

    # Let go
    gripper_if.open()
def main():
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
    required = parser.add_argument_group('required arguments')
    required.add_argument(
        '-l', '--limb', required=True, choices=['left', 'right'],
        help='send joint trajectory to which limb'
    )

    args = parser.parse_args(rospy.myargv()[1:])
    print args
    limb = args.limb

    print("Initializing node... ")
    rospy.init_node("super_stacker_%s" % (limb,))
    print("Getting robot state... ")
    rs = baxter_interface.RobotEnable(CHECK_VERSION)
    print("Enabling robot... ")
    rs.enable()

    #Calibrate gripper
    gripper_if = baxter_interface.Gripper(limb)
    if not gripper_if.calibrated():
        print "Calibrating gripper"
        gripper_if.calibrate()

    limbInterface = baxter_interface.Limb(limb)

    iksvc, ns = ik_command.connect_service(limb)
    rate = rospy.Rate(100)

    # Get goal poses of each object in the scene
    dc = DepthCaller(limb)

    # Subscribe to estimate_depth
    # Move to pose published by estimate_depth
    while (not dc.done) and (not rospy.is_shutdown()):
        rate.sleep()
        #pass
    
    
    # Subscribe to object_finder and start visual servoing/grasping
   
    # The stack pose is the pose of the smallest object with a z-offset
    # accounting for the height of the object (this assumption makes
    # it really important that the segmentation is accurate)
    stack_pose = [0.594676466827,     -0.296644499519,     -0.0322744943164,     0.971805911045,     -0.22637170004,     0.065946440385,     0.000437813100735]
    #stack_pose = dc.object_poses[-1]
    #stack_pose = incrementPoseZ(stack_pose, object_height)
    #dc.object_poses.pop(len(dc.object_poses)-1)

    for pose in dc.object_poses:

        ik_command.service_request(iksvc, pose, limb, blocking=True)
        vc = VisualCommand(iksvc, limb)
        vc.subscribe()

        while (not vc.done) and (not rospy.is_shutdown()):
            rate.sleep()

        ik_command.service_request(iksvc, stack_pose, limb, blocking=True)
        #limbInterface.move_to_joint_positions(stack_pose)

        # Let go
        gripper_if.open(block=True)

        stack_pose = incrementPoseZ(pose, object_height)
Exemple #4
0
def main():
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
    required = parser.add_argument_group('required arguments')
    required.add_argument('-l',
                          '--limb',
                          required=True,
                          choices=['left', 'right'],
                          help='send joint trajectory to which limb')

    args = parser.parse_args(rospy.myargv()[1:])
    limb = args.limb

    rospy.init_node('stackit')

    iksvc, ns = ik_command.connect_service(limb)
    moveit_commander.roscpp_initialize(sys.argv)

    rate = rospy.Rate(1)

    robot = moveit_commander.RobotCommander()
    scene = moveit_commander.PlanningSceneInterface()
    group = moveit_commander.MoveGroupCommander(limb + "_arm")
    group.allow_replanning(True)

    print("Getting robot state... ")
    rs = baxter_interface.RobotEnable(CHECK_VERSION)
    print("Enabling robot... ")
    rs.enable()

    #Calibrate gripper
    gripper_if = baxter_interface.Gripper(limb)
    if not gripper_if.calibrated():
        print "Calibrating gripper"
        gripper_if.calibrate()

    obj_manager = ObjectManager()
    while len(obj_manager.collision_objects) <= 0:
        rate.sleep()
    objects = obj_manager.collision_objects

    object_height = params['object_height']
    """if len(objects) > 1:
        stack_pose = projectPose(objects[len(objects)-1].primitive_poses[0])
        stack_pose = incrementPoseMsgZ(stack_pose, object_height*2.0)
        objects.pop(len(objects)-1)
    elif len(objects) == 1:
        stack_pose = projectPose(objects[0].primitive_poses[0])"""
    stack_pose = Pose(position=Point(0.593, -0.212, object_height - 0.130),
                      orientation=Quaternion(0.6509160466, 0.758886809948,
                                             -0.0180992582839,
                                             -0.0084573527776))

    processed_win = "Processed image"
    raw_win = "Hand camera"
    cv2.namedWindow(processed_win)
    #cv2.namedWindow(raw_win)

    rospy.on_shutdown(obj_manager.remove_known_objects)

    for obj in objects:
        obj_pose = obj.primitive_poses[0]

        #group.pick(obj.id)
        #group.place(obj.id, stack_pose)

        print "Got pose:", obj.primitive_poses[0]
        pose = projectPose(obj.primitive_poses[0])
        pose = incrementPoseMsgZ(pose, object_height * 1.7)
        print "Modified pose:", pose
        #if obj.id in obj_manager.id_operations:
        #    obj_manager.id_operations[obj.id] = CollisionObject.REMOVE
        #obj_manager.publish(obj)

        print "setting target to pose"
        # Move to the next block
        group.clear_pose_targets()
        group.set_start_state_to_current_state()
        group.set_pose_target(pose)

        plan = group.plan()

        # is there a better way of checking this?
        plan_found = len(plan.joint_trajectory.points) > 0

        if plan_found:
            #print "============ Waiting while RVIZ displays plan1..."
            #rospy.sleep(3)
            group.go(wait=True)

            imgproc = ObjectFinder("star", None, None)
            imgproc.subscribe("/cameras/" + limb + "_hand_camera/image")
            imgproc.publish(limb)

            vc = VisualCommand(iksvc, limb)
            vc.subscribe()

            while (not vc.done) and (not rospy.is_shutdown()):
                blobArray = []

                for centroid, axis in zip(imgproc.centroids, imgproc.axes):
                    blob = BlobInfo()
                    centroid = Point(*centroid)
                    blob.centroid = centroid
                    if axis is None:
                        axis = -1 * numpy.ones(6)
                    blob.axis = Polygon([
                        Point(*axis[0:3].tolist()),
                        Point(*axis[3:6].tolist())
                    ])
                    blobArray.append(blob)

                msg = BlobInfoArray()
                msg.blobs = blobArray
                imgproc.handler_pub.publish(msg)
                if imgproc.processed is not None:
                    cv2.imshow(processed_win, imgproc.processed)
                cv2.waitKey(10)
            vc.unsubscribe()
            imgproc.unsubscribe()

            print "Adding attached message"
            #Add attached message
            #obj.primitive_poses[0] = incrementPoseMsgZ(obj.primitive_poses[0], *object_height) #this is in the base frame...?
            obj_manager.publish_attached(obj, limb)
            #touch_links = [limb+"_gripper", limb+"_gripper_base", limb+"_hand_camera", limb+"_hand_range"]
            #group.attach_object(obj.id, limb+"_gripper", touch_links = touch_links)
            # Carefully rise away from the object before we plan another path
            pose = incrementPoseMsgZ(pose, 2 * object_height)  # test this
            ik_command.service_request_pose(iksvc, pose, limb, blocking=True)

        else:
            print "Unable to plan path"
            continue
            # what to do?

        # Move to the stacking position
        group.clear_pose_targets()
        group.set_start_state_to_current_state()
        group.set_pose_target(stack_pose)
        plan = group.plan()
        plan_found = len(plan.joint_trajectory.points) > 0

        if plan_found:
            #print "============ Waiting while RVIZ displays plan2..."
            #rospy.sleep(3)
            group.go(wait=True)
            gripper_if.open(block=True)
            group.detach_object(obj.id)
            # Carefully rise away from the object before we plan another path
            pose = incrementPoseMsgZ(stack_pose, 2 * object_height)
            ik_command.service_request_pose(iksvc, pose, limb, blocking=True)

        obj.operation = CollisionObject.REMOVE
        obj_manager.publish(obj)
        # Get the next stack pose
        stack_pose = incrementPoseMsgZ(stack_pose, object_height * 3 / 4)
        """if obj.id in obj_manager.id_operations: