Пример #1
0
    def __init__(self):
        # Sets the arm that is used for the servoing
        limb = 'right'
        self._baxter = BaxterVS(limb)

        # Initializes the marker that the arm should track
        target_marker = 0
        self._apriltag_client = AprilTagClient(target_marker)

        self._visual_servo = VisualServoing(False)
        cv2.namedWindow("Detected tags", 0)
Пример #2
0
    def __init__(self):
        # Baxter specific code. You don't need this if you're using another robot.
        # Sets the arm that is used for the servoing
        limb = 'right'
        self._baxter = BaxterVS(limb)

        # AprilTag specific code. You don't need this if you're using another tracking system.
        # Initializes the marker that the arm should track
        target_marker = 0
        self._apriltag_client = AprilTagClient(target_marker)

        self._visual_servo = VisualServoing(ibvs=True)
Пример #3
0
    def __init__(self):
        # Baxter specific code. You don't need this if you're using another robot.
        # Sets the arm that is used for the servoing
        limb='left'
        self._baxter = BaxterVS(limb)

        self.gripper = baxter_interface.Gripper(limb)
        self.gripper.calibrate()

        self.golf_ball_x = 0.85                        # x     = front back
        self.golf_ball_y = 0.10                        # y     = left right
        self.golf_ball_z = 0.0                        # z     = up down
        self.roll        = -1.0 * math.pi              # roll  = horizontal
        self.pitch       = 0.0 * math.pi               # pitch = vertical
        self.yaw         = 0.0 * math.pi               # yaw   = rotation

        self.pose = (self.golf_ball_x,
                        self.golf_ball_y,
                        self.golf_ball_z,
                        self.roll,
                        self.pitch,
                        self.yaw)
        print 'ready to move'
        self.baxter_ik_move(limb, self.pose)
        # self.golf_ball_x = self.golf_ball_x + 0.010
        # # self.golf_ball_y = self.golf_ball_y - 0.10
        # # self.golf_ball_z = self.golf_ball_z + 0.10  
        # self.pitch = self.pitch - 0.2 * math.pi 
        # self.pose = (self.golf_ball_x,
        #                 self.golf_ball_y,
        #                 self.golf_ball_z,
        #                 self.roll,
        #                 self.pitch,
        #                 self.yaw)
        # print 'ready to move'
        # self.baxter_ik_move(limb, self.pose)          
        # AprilTag specific code. You don't need this if you're using another tracking system.
        # Initializes the marker that the arm should track
        target_marker = 0
        # self._apriltag_client = AprilTagClient(target_marker)

        self._visual_servo = VisualServoing(ibvs=True)

        # camera parameters (NB. other parameters in open_camera)
        self.cam_calib    = 0.0025                     # meters per pixel at 1 meter
        self.cam_x_offset = -0.01                   # camera gripper offset
        self.cam_y_offset = -0.035
        self.width        = 960                       # Camera resolution
        self.height       = 600

        # callback image
        self.cv_image = cv.CreateImage((self.width, self.height), 8, 3)

        # create image publisher to head monitor
        self.pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
        self.subscribe_to_camera(limb)
Пример #4
0
    def __init__(self):
        # Sets the arm that is used for the servoing
        limb='right'
        self._baxter = BaxterVS(limb)

        # Initializes the marker that the arm should track
        target_marker = 0
        self._apriltag_client = AprilTagClient(target_marker)

        self._visual_servo = VisualServoing(False)    
        cv2.namedWindow("Detected tags",0)
Пример #5
0
    def __init__(self):
        # Baxter specific code. You don't need this if you're using another robot.
        # Sets the arm that is used for the servoing
        limb='right'
        self._baxter = BaxterVS(limb)

        # AprilTag specific code. You don't need this if you're using another tracking system.
        # Initializes the marker that the arm should track
        target_marker = 0
        self._apriltag_client = AprilTagClient(target_marker)
        
        self._visual_servo = VisualServoing(ibvs=True)
Пример #6
0
    def __init__(self):
        # Sets the arm that is used for the servoing
        limb='left'
        self._baxter = BaxterVS(limb)

        self.gripper = baxter_interface.Gripper(limb)
        self.gripper.calibrate()

        # Initializes the marker that the arm should track
        target_marker = 0
        # self._apriltag_client = AprilTagClient(target_marker)

        self._visual_servo = VisualServoing(False)    
        # cv2.namedWindow("Detected tags",0)

        self.marker_t=None
        self.marker_R=None

        self.target_obj_name = "expo_dry_erase_board_eraser"

        simtrack_topic = "/simtrack/" + self.target_obj_name
        print 'simtrack_topic: ', simtrack_topic
        self.subscribe_to_simtrack(simtrack_topic)
Пример #7
0
class IbvsEih(object):
    """
    Performs eye in hand (eih) image based visual servoing (ibvs). 
    """
    def __init__(self):
        # Baxter specific code. You don't need this if you're using another robot.
        # Sets the arm that is used for the servoing
        limb='left'
        self._baxter = BaxterVS(limb)

        self.golf_ball_x = 0.8                        # x     = front back
        self.golf_ball_y = 0.25                        # y     = left right
        self.golf_ball_z = -0.1                        # z     = up down
        self.roll        = -1.0 * math.pi              # roll  = horizontal
        self.pitch       = 0.0 * math.pi               # pitch = vertical
        self.yaw         = 0.0 * math.pi               # yaw   = rotation

        self.pose = (self.golf_ball_x,
                        self.golf_ball_y,
                        self.golf_ball_z,
                        self.roll,
                        self.pitch,
                        self.yaw)
        print 'ready to move'
        self.baxter_ik_move(limb, self.pose)
        # AprilTag specific code. You don't need this if you're using another tracking system.
        # Initializes the marker that the arm should track
        target_marker = 0
        self._apriltag_client = AprilTagClient(target_marker)

        self._visual_servo = VisualServoing(ibvs=True)

        # camera parameters (NB. other parameters in open_camera)
        self.cam_calib    = 0.0025                     # meters per pixel at 1 meter
        self.cam_x_offset = -0.01                   # camera gripper offset
        self.cam_y_offset = -0.035
        self.width        = 960                       # Camera resolution
        self.height       = 600

        # callback image
        self.cv_image = cv.CreateImage((self.width, self.height), 8, 3)

        # create image publisher to head monitor
        self.pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
        self.subscribe_to_camera(limb)
    
    def new_image_arrived(self):
        """
        Boolean to test if a new image has arrived.
        """
        print "waiting for new image"
        if self._apriltag_client.corners is not None:
            self._apriltag_client.corners = None
            return True
        return False

    # def _get_detected_corners(self):
    #     """
    #     Returns the most recently detected corners in the image.
    #     """
    #     # This method currently uses AprilTag detections, so replace with
    #     # your own method if otherwise.
    #     # print "looking for corners"
    #     return self._apriltag_client.corners
    
    def _command_velocity(self,cam_vel,error_norm):
        """
        Move the camera at the specified v and omega in vel (6x1 vector).
        """
        # This method currently commands the Baxter robot from Rethink Robotics.
        #Replace with your own method if using another manipulator.
        
        hand_vel_in_base = self._baxter.cam_to_body(cam_vel)
        # print 'baxter_vel: ', baxter_vel
        self._baxter.set_hand_vel(hand_vel_in_base,error_norm)
    
    def set_target(self,final_camera_depth,desired_corners):
        """
        Sets the final camera depth (Z) and desired position for the tracked features
        at the goal position.
        """


        Z = final_camera_depth
        ideal_cam_pose = np.array([0,0,Z])  
        # print 'ideal_cam_pose: ', ideal_cam_pose
        self._visual_servo.set_target(ideal_cam_pose,None,desired_corners)
    
    def move_to_position(self,final_camera_depth,desiredImg, desiredCircles, desired_centers, dist_tol):
        """
        Runs one instance of the visual servoing control law. Call when a new
        image has arrived.
        """
        
        # self.set_target(final_camera_depth,desired_corners)
        # r = rospy.Rate(60)

        error_norm = 1000
        while error_norm>dist_tol and not rospy.is_shutdown():
            # if not self.new_image_arrived():
            #     # print " no new_image_arrived"
            #     continue
            
            # Continue if no corners detected
            # marker_corners = self._get_detected_corners()
            # img_array = cv2array(self.cv_image)
            # print self.cv_image.shape
            # cv.WaitKey(1000)
            print type(self.cv_image)
            # currentI = cv2array(self.cv_image)
            currentI = numpy.asarray(self.cv_image[:,:])

            print type(currentI)
            gray = cv2.cvtColor(currentI, cv2.COLOR_BGR2GRAY)
            # cv2.imshow("input", gray)
            # cv2.waitKey(0)

            # detect circles in the image
            currentCircles = cv2.HoughCircles(gray, cv2.cv.CV_HOUGH_GRADIENT, 2, 60)

            # print 'desiredCircles: ', desiredCircles
            # print 'currentCircles: ', currentCircles

            FoundThreeCircles = False

            if currentCircles is not None:
                # convert the (x, y) coordinates and radius of the circles to integers
                currentCircles = np.round(currentCircles[0, :]).astype("int")

                number_of_circles =3
                circle_x = np.zeros(number_of_circles)
                circle_y = np.zeros(number_of_circles)
                current_centers = np.zeros(number_of_circles*2)
                i = 0
                for (x, y, r) in currentCircles:
                    print 'r: ', r
                    if r < 1100 and r > 30 :
                        # draw the circle in the output image, then draw a rectangle
                        # corresponding to the center of the circle
                        cv2.circle(currentI, (x, y), r, (0, 255, 0), 4)
                        cv2.rectangle(currentI, (x - 5, y - 5), (x + 5, y + 5), (0, 128, 255), -1)
                        circle_x[i] = x
                        circle_y[i] = y
                        i = i+1
                        if i == number_of_circles:
                            FoundThreeCircles = True
                            break

                index = np.argsort(circle_x)
                for i in range(0,number_of_circles):
                    current_centers[2*i] = circle_x[index[i]]      
                    current_centers[2*i+1] = circle_y[index[i]]

                print 'desired_centers: ', desired_centers
                print 'current_centers: ', current_centers 

            
            # plt.figure(1);
            # plt.show(plt.imshow(img))
            msg = cv_bridge.CvBridge().cv2_to_imgmsg(currentI, encoding="bgr8")
            self.pub.publish(msg)
            # plt.figure(1);
            # plt.show(plt.imshow(img))


            # show the output image
            cv2.imshow("output", currentI)
            # cv2.waitKey(0)

            # ratio=0.75
            # reprojThresh=4.0
            # # print 'desiredkps_npa: ', desiredkps_npa
            # # print 'desiredFeatures: ', desiredFeatures
            # (matches, H, status) = self.matchKeypoints(desiredkps_npa, detectedkps_npa, desiredFeatures, detectedFeatures, ratio, reprojThresh)
            # selected_matches_id = np.where(status != 0)[0]
            # if matches is None:
            #     # print "no marker corners"
            #     continue
            if not FoundThreeCircles:
                print "no enough matching"
                continue

            # selected_matches_id = selected_matches_id[0:3]

            # print 'mathces:', len(selected_matches_id)
            # print 'desiredkps_npa: ', desiredkps_npa
            # print 'status: ', status

            # cv2.waitKey(0)

            # print desiredkps_npa.shape
            # print detectedkps_npa.shape
            # print detectedkps_npa
            # print 'mathces:', matches
            # print 'mathces[1]:', matches[1][0]

            # desired_corners = np.zeros(len(selected_matches_id)*2)
            # detected_corners = np.zeros(len(selected_matches_id)*2)
            # # selected_matches = [(0,0),(0,0),(0,0)]

            # for i in range(0,len(selected_matches_id)):
            #     desired_m = desiredkps_npa[matches[selected_matches_id[i]][1]]
            #     desired_corners[2*i] = desired_m[0]
            #     desired_corners[2*i+1] = desired_m[1]
            #     detected_m = detectedkps_npa[matches[selected_matches_id[i]][0]]
            #     detected_corners[2*i] = detected_m[0]
            #     detected_corners[2*i+1] = detected_m[1]
            #     # selected_matches[i] = matches[selected_matches_id[i]] 

            # print 'desired_corners: ', desired_corners
            # print 'detected_corners: ', detected_corners
            # vis = self.drawMatches(desiredImg, currentI, desiredkps_npa, detectedkps_npa, matches, status)
            # print 'selected_matches: ', selected_matches

            



            # Don't move if the target hasn't been set
            if not self._visual_servo._target_set:
                self.set_target(final_camera_depth,desired_centers)
                print "the target hasn't been set"
                continue
            
            # Get control law velocity and transform to body frame, then send to robot
            cam_vel, error = self._visual_servo.get_next_vel(corners = current_centers)
            error_norm = np.linalg.norm(error)
            self._command_velocity(cam_vel, error_norm)
            # print 'servo_vel: ', servo_vel
            # print 'before sleep'
            
            # print rospy.is_shutdown()
            # r.sleep()
            # cv.WaitKey(1000)
            # vis = self.drawMatches(desiredImg, currentI, desiredkps_npa, detectedkps_npa, matches, status)
            # # vis = -self.drawMatches(desiredImg, currentI, desiredkps_npa, detectedkps_npa, selected_matches, np.ones(3))
            # cv2.imshow("Keypoint Matches", vis)
            # cv2.waitKey(0)

            rospy.sleep(0.8)




    def baxter_ik_move(self, limb, rpy_pose):
        quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base")

        node = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
        ik_service = rospy.ServiceProxy(node, SolvePositionIK)
        ik_request = SolvePositionIKRequest()
        hdr = Header(stamp=rospy.Time.now(), frame_id="base")

        ik_request.pose_stamp.append(quaternion_pose)
        try:
            rospy.wait_for_service(node, 5.0)
            ik_response = ik_service(ik_request)
        except (rospy.ServiceException, rospy.ROSException), error_message:
            rospy.logerr("Service request failed: %r" % (error_message,))
            sys.exit("ERROR - baxter_ik_move - Failed to append pose")

        if ik_response.isValid[0]:
            print("PASS: Valid joint configuration found")
            # convert response to joint position control dictionary
            limb_joints = dict(zip(ik_response.joints[0].name, ik_response.joints[0].position))
            # move limb
            # if self.limb == limb:
            limb_interface = baxter_interface.Limb(limb)
            limb_interface.move_to_joint_positions(limb_joints)
            print 'moving'
            # else:
                # self.other_limb_interface.move_to_joint_positions(limb_joints)
        else:
            # display invalid move message on head display
            self.splash_screen("Invalid", "move")
            # little point in continuing so exit with error message
            print "requested move =", rpy_pose
            sys.exit("ERROR - baxter_ik_move - No valid joint configuration found")

        # if self.limb == limb:               # if working arm
        quaternion_pose = limb_interface.endpoint_pose()
        position        = quaternion_pose['position']

        # if working arm remember actual (x,y) position achieved
        self.pose = [position[0], position[1],                                \
                     self.pose[2], self.pose[3], self.pose[4], self.pose[5]]
Пример #8
0
class PbvsEih(object):
    """
    Performs eye in hand (eih) image based visual servoing (ibvs). 
    """
    def __init__(self):
        # Sets the arm that is used for the servoing
        limb='left'
        self._baxter = BaxterVS(limb)

        self.gripper = baxter_interface.Gripper(limb)
        self.gripper.calibrate()

        # Initializes the marker that the arm should track
        target_marker = 0
        # self._apriltag_client = AprilTagClient(target_marker)

        self._visual_servo = VisualServoing(False)    
        # cv2.namedWindow("Detected tags",0)

        self.marker_t=None
        self.marker_R=None

        self.target_obj_name = "expo_dry_erase_board_eraser"

        simtrack_topic = "/simtrack/" + self.target_obj_name
        print 'simtrack_topic: ', simtrack_topic
        self.subscribe_to_simtrack(simtrack_topic)
  

    def get_pose(self, data):
        # print 'cam_pose: ', data.pose
        (self.marker_t, self.marker_R) = get_t_R(data.pose)

    def subscribe_to_simtrack(self, simtrack_topic):
        print '\n subscribing'
        sub = rospy.Subscriber(simtrack_topic, PoseStamped, self.get_pose)
        print '\n end subscribing'

    def _main_iter(self):
        """
        Runs one instance of the visual servoing control law. Call in a loop.
        """
        # key = -2
        # image = self._apriltag_client.image
        # if image is None:
        #     print 'no image'
        #     return
        # Draw the ideal position if selected
        # if self._visual_servo._target_set:
        #     print 'target set'
        #     corners = self._visual_servo._ideal_corners
        #     for i in range(0,4):
        #         # Camera intrinsics currently hard coded, tune as needed, 
        #         # or add a subscriber to the camera_info message.
        #         cv2.circle(image,(int(corners[i*2]*407.19+317.22),int(corners[i*2+1]*407.195786+206.752)),5,(255,0,0),5)
        # cv2.imshow("Detected tags",image)
        # key = cv2.waitKey(5)
        
        # pose: 
        #   position: 
        #     x: 0.0703210349954
        #     y: 0.0207515639124
        #     z: 0.100353679031
        #   orientation: 
        #     x: 0.0305494043199
        #     y: 0.996535070586
        #     z: 0.0757659684332
        #     w: 0.0156238604684

          # position: 
          #   x: 0.125581991683
          #   y: 0.056925803143
          #   z: 0.167849532136
          # orientation: 
          #   x: 0.00578534021301
          #   y: 0.999924139472
          #   z: 0.00808789160022
          #   w: 0.00726850397684


        # pose: 
        #   position: 
        #     x: 0.0798907744327
        #     y: 0.0393022484119
        #     z: 0.107615683339
        #   orientation: 
        #     x: 0.0300291017366
        #     y: 0.999237092819
        #     z: 0.0247878443813
        #     w: 0.00300801503595
        Success = False
        error_norm = 1
        error_y = 1
        dist_tol = 0.008
        DesiredPose = Pose()
        DesiredPose.position.x = .125581991683
        DesiredPose.position.y = .056925803143
        DesiredPose.position.z = .167849532136
        DesiredPose.orientation.x = 0.00578534021301
        DesiredPose.orientation.y = 0.999924139472
        DesiredPose.orientation.z = 0.00808789160022
        DesiredPose.orientation.w = 0.00726850397684
        while error_norm > dist_tol or error_y > 0.005 :

            # Return if no tag detected
            marker_pose = self.marker_t
            
            marker_rot = self.marker_R
            # marker_corners = self._apriltag_client.corners
            print 'marker_pose: ', marker_pose
            # print 'marker_rot: ', marker_rot
            if marker_pose is None:
                return

            # Don't reprocess detections
            # if self._visual_servo._target_set:
            #     self._apriltag_client.marker_t = None
            #     self._apriltag_client.marker_R = None
            #     self._apriltag_client.corners = None
            
                # Press any key to set a new target position for the servo control law.
            # print 'key: ', key
            if not self._visual_servo._target_set: #key !=-1:
                rospy.loginfo("Setting new target")
                print 'desired_pose: ', marker_pose
                # desired_corners = marker_corners
                # desired_pose = marker_pose
                # desired_rot = marker_rot
                (desired_pose, desired_rot) = get_t_R(DesiredPose)
                self._visual_servo.set_target(desired_pose,desired_rot)
                # self._visual_servo.set_target(desired_pose,desired_rot,desired_corners)

            if not self._visual_servo._target_set:
                return

            # Get control law velocity and transform to body frame, then send to baxter
            servo_vel, error = self._visual_servo.get_next_vel(marker_pose,marker_rot)
            error_norm = np.linalg.norm(error)/6
            print 'error_norm: ', error_norm
            # print 'servo_vel: ', servo_vel
            baxter_vel = self._baxter.cam_to_body(servo_vel)
            # print 'baxter_vel: ', baxter_vel
            self._baxter.set_hand_vel(baxter_vel, error_norm)
            error_y = np.abs(marker_pose[1]-DesiredPose.position.y)
            print 'error_y: ', error_y
            rospy.sleep(0.5)

        hand_pose = baxter.get_arm_pose('left')
        print 'hand_pose:', hand_pose

        self.golf_ball_x = hand_pose.position.x                      # x     = front back
        self.golf_ball_y = hand_pose.position.y                       # y     = left right
        self.golf_ball_z = hand_pose.position.z -0.02                       # z     = up down
        self.orientation = hand_pose.orientation

        self.pose = (self.golf_ball_x,
                        self.golf_ball_y,
                        self.golf_ball_z,
                        self.orientation.x,
                        self.orientation.y,
                        self.orientation.z,
                        self.orientation.w)

        print 'ready to move'
        self.baxter_ik_move('left', self.pose)

        self.golf_ball_z = self.golf_ball_z -0.02                       # z     = up down
        self.pose = (self.golf_ball_x,
                        self.golf_ball_y,
                        self.golf_ball_z,
                        self.orientation.x,
                        self.orientation.y,
                        self.orientation.z,
                        self.orientation.w)

        print 'ready to move'
        self.baxter_ik_move('left', self.pose)

        self.golf_ball_z = self.golf_ball_z -0.02                       # z     = up down
        self.pose = (self.golf_ball_x,
                        self.golf_ball_y,
                        self.golf_ball_z,
                        self.orientation.x,
                        self.orientation.y,
                        self.orientation.z,
                        self.orientation.w)

        print 'ready to move'
        self.baxter_ik_move('left', self.pose)

        rospy.sleep(1)
        self.gripper.close()
        rospy.sleep(1)

        hand_pose = baxter.get_arm_pose('left')
        self.golf_ball_x = hand_pose.position.x                       # x     = front back
        self.golf_ball_y = hand_pose.position.y                       # y     = left right
        self.golf_ball_z = hand_pose.position.z+0.03                       # z     = up down
        self.orientation = hand_pose.orientation

        self.pose = (self.golf_ball_x,
                        self.golf_ball_y,
                        self.golf_ball_z,
                        self.orientation.x,
                        self.orientation.y,
                        self.orientation.z,
                        self.orientation.w)

        print 'ready to move'
        self.baxter_ik_move('left', self.pose)
        Success = True
        return Success

    def baxter_ik_move(self, limb, rpy_pose):
        quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base")

        node = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
        ik_service = rospy.ServiceProxy(node, SolvePositionIK)
        ik_request = SolvePositionIKRequest()
        hdr = Header(stamp=rospy.Time.now(), frame_id="base")

        ik_request.pose_stamp.append(quaternion_pose)
        try:
            rospy.wait_for_service(node, 5.0)
            ik_response = ik_service(ik_request)
        except (rospy.ServiceException, rospy.ROSException), error_message:
            rospy.logerr("Service request failed: %r" % (error_message,))
            sys.exit("ERROR - baxter_ik_move - Failed to append pose")

        if ik_response.isValid[0]:
            print("PASS: Valid joint configuration found")
            # convert response to joint position control dictionary
            limb_joints = dict(zip(ik_response.joints[0].name, ik_response.joints[0].position))
            # move limb
            # if self.limb == limb:
            limb_interface = baxter_interface.Limb(limb)
            limb_interface.move_to_joint_positions(limb_joints)
            print 'moving'
            # else:
                # self.other_limb_interface.move_to_joint_positions(limb_joints)
        else:
            # display invalid move message on head display
            # self.splash_screen("Invalid", "move")
            # little point in continuing so exit with error message
            print "requested move =", rpy_pose
            sys.exit("ERROR - baxter_ik_move - No valid joint configuration found")

        # if self.limb == limb:               # if working arm
        quaternion_pose = limb_interface.endpoint_pose()
        position        = quaternion_pose['position']

        # if working arm remember actual (x,y) position achieved
        self.pose = [position[0], position[1],                                \
                     self.pose[2], self.pose[3], self.pose[4], self.pose[5]]
Пример #9
0
class PbvsEih(object):
    """
    Performs eye in hand (eih) image based visual servoing (ibvs). 
    """
    def __init__(self):
        # Sets the arm that is used for the servoing
        limb = 'right'
        self._baxter = BaxterVS(limb)

        # Initializes the marker that the arm should track
        target_marker = 0
        self._apriltag_client = AprilTagClient(target_marker)

        self._visual_servo = VisualServoing(False)
        cv2.namedWindow("Detected tags", 0)

    def _main_iter(self):
        """
        Runs one instance of the visual servoing control law. Call in a loop.
        """
        key = -1
        image = self._apriltag_client.image
        if image is None:
            return
        # Draw the ideal position if selected
        if self._visual_servo._target_set:
            corners = self._visual_servo._ideal_corners
            for i in range(0, 4):
                # Camera intrinsics currently hard coded, tune as needed,
                # or add a subscriber to the camera_info message.
                cv2.circle(image,
                           (int(corners[i * 2] * 407.19 + 317.22),
                            int(corners[i * 2 + 1] * 407.195786 + 206.752)), 5,
                           (255, 0, 0), 5)
        cv2.imshow("Detected tags", image)
        key = cv2.waitKey(5)

        # Return if no tag detected
        marker_pose = self._apriltag_client.marker_t
        marker_rot = self._apriltag_client.marker_R
        marker_corners = self._apriltag_client.corners
        if marker_pose is None:
            return

        # Don't reprocess detections
        if self._visual_servo._target_set:
            self._apriltag_client.marker_t = None
            self._apriltag_client.marker_R = None
            self._apriltag_client.corners = None

            # Press any key to set a new target position for the servo control law.
        if key != -1:
            rospy.loginfo("Setting new target")
            desired_corners = marker_corners
            desired_pose = marker_pose
            desired_rot = marker_rot
            self._visual_servo.set_target(desired_pose, desired_rot,
                                          desired_corners)

        if not self._visual_servo._target_set:
            return

        # Get control law velocity and transform to body frame, then send to baxter
        servo_vel = self._visual_servo.get_next_vel(marker_pose, marker_rot)
        baxter_vel = self._baxter.cam_to_body(servo_vel)
        self._baxter.set_hand_vel(baxter_vel)
Пример #10
0
class PbvsEih(object):
    """
    Performs eye in hand (eih) image based visual servoing (ibvs). 
    """
    def __init__(self):
        # Sets the arm that is used for the servoing
        limb='right'
        self._baxter = BaxterVS(limb)

        # Initializes the marker that the arm should track
        target_marker = 0
        self._apriltag_client = AprilTagClient(target_marker)

        self._visual_servo = VisualServoing(False)    
        cv2.namedWindow("Detected tags",0)
                    
    def _main_iter(self):
        """
        Runs one instance of the visual servoing control law. Call in a loop.
        """
        key = -1
        image = self._apriltag_client.image
        if image is None:
            return
        # Draw the ideal position if selected
        if self._visual_servo._target_set:
            corners = self._visual_servo._ideal_corners
            for i in range(0,4):
                # Camera intrinsics currently hard coded, tune as needed, 
                # or add a subscriber to the camera_info message.
                cv2.circle(image,(int(corners[i*2]*407.19+317.22),int(corners[i*2+1]*407.195786+206.752)),5,(255,0,0),5)
        cv2.imshow("Detected tags",image)
        key = cv2.waitKey(5)
        
        # Return if no tag detected
        marker_pose = self._apriltag_client.marker_t
        marker_rot = self._apriltag_client.marker_R
        marker_corners = self._apriltag_client.corners
        if marker_pose is None:
            return

        # Don't reprocess detections
        if self._visual_servo._target_set:
            self._apriltag_client.marker_t = None
            self._apriltag_client.marker_R = None
            self._apriltag_client.corners = None
        
            # Press any key to set a new target position for the servo control law.
        if key !=-1:
            rospy.loginfo("Setting new target")
            desired_corners = marker_corners
            desired_pose = marker_pose
            desired_rot = marker_rot
            self._visual_servo.set_target(desired_pose,desired_rot,desired_corners)

        if not self._visual_servo._target_set:
            return

        # Get control law velocity and transform to body frame, then send to baxter
        servo_vel = self._visual_servo.get_next_vel(marker_pose,marker_rot)
        baxter_vel = self._baxter.cam_to_body(servo_vel)
        self._baxter.set_hand_vel(baxter_vel)
Пример #11
0
class IbvsEih(object):
    """
    Performs eye in hand (eih) image based visual servoing (ibvs). 
    """
    def __init__(self):
        # Baxter specific code. You don't need this if you're using another robot.
        # Sets the arm that is used for the servoing
        limb='right'
        self._baxter = BaxterVS(limb)

        # AprilTag specific code. You don't need this if you're using another tracking system.
        # Initializes the marker that the arm should track
        target_marker = 0
        self._apriltag_client = AprilTagClient(target_marker)
        
        self._visual_servo = VisualServoing(ibvs=True)
    
    def new_image_arrived(self):
        """
        Boolean to test if a new image has arrived.
        """
        if self._apriltag_client.corners is not None:
            self._apriltag_client.corners = None
            return True
        return False

    def _get_detected_corners(self):
        """
        Returns the most recently detected corners in the image.
        """
        # This method currently uses AprilTag detections, so replace with
        # your own method if otherwise.

        return self._apriltag_client.corners
    
    def _command_velocity(self,vel):
        """
        Move the camera at the specified v and omega in vel (6x1 vector).
        """
        # This method currently commands the Baxter robot from Rethink Robotics.
        #Replace with your own method if using another manipulator.
        
        baxter_vel = self._baxter.cam_to_body(servo_vel)
        self._baxter.set_hand_vel(baxter_vel)
    
    def set_target(self,final_camera_depth,desired_corners):
        """
        Sets the final camera depth (Z) and desired position for the tracked features
        at the goal position.
        """
        ideal_cam_pose = np.array([0,0,Z])
        self._vision_servo.set_target(ideal_cam_pose,None,desired_corners)
    
    def move_to_position(self,final_camera_depth,desired_corners,dist_tol):
        """
        Runs one instance of the visual servoing control law. Call when a new
        image has arrived.
        """
        self.set_target(final_camera_depth,desired_corners)
        r = rospy.Rate(60)
        while np.linalg.norm(error)>dist_tol and not rospy.is_shutdown():
            if not self.new_image_arrived():
                continue
            
            # Continue if no corners detected
            marker_corners = self._get_detected_corners()
            if marker_corners is None:
                continue

            # Don't move if the target hasn't been set
            if not self._visual_servo._target_set:
                continue
            
            # Get control law velocity and transform to body frame, then send to robot
            servo_vel = self._visual_servo.get_next_vel(corners = marker_corners)
            self._command_velocity(servo_vel)
            r.sleep()
Пример #12
0
class IbvsEih(object):
    """
    Performs eye in hand (eih) image based visual servoing (ibvs). 
    """
    def __init__(self):
        # Baxter specific code. You don't need this if you're using another robot.
        # Sets the arm that is used for the servoing
        limb = 'right'
        self._baxter = BaxterVS(limb)

        # AprilTag specific code. You don't need this if you're using another tracking system.
        # Initializes the marker that the arm should track
        target_marker = 0
        self._apriltag_client = AprilTagClient(target_marker)

        self._visual_servo = VisualServoing(ibvs=True)

    def new_image_arrived(self):
        """
        Boolean to test if a new image has arrived.
        """
        if self._apriltag_client.corners is not None:
            self._apriltag_client.corners = None
            return True
        return False

    def _get_detected_corners(self):
        """
        Returns the most recently detected corners in the image.
        """
        # This method currently uses AprilTag detections, so replace with
        # your own method if otherwise.

        return self._apriltag_client.corners

    def _command_velocity(self, vel):
        """
        Move the camera at the specified v and omega in vel (6x1 vector).
        """
        # This method currently commands the Baxter robot from Rethink Robotics.
        #Replace with your own method if using another manipulator.

        baxter_vel = self._baxter.cam_to_body(servo_vel)
        self._baxter.set_hand_vel(baxter_vel)

    def set_target(self, final_camera_depth, desired_corners):
        """
        Sets the final camera depth (Z) and desired position for the tracked features
        at the goal position.
        """
        ideal_cam_pose = np.array([0, 0, Z])
        self._vision_servo.set_target(ideal_cam_pose, None, desired_corners)

    def move_to_position(self, final_camera_depth, desired_corners, dist_tol):
        """
        Runs one instance of the visual servoing control law. Call when a new
        image has arrived.
        """
        self.set_target(final_camera_depth, desired_corners)
        r = rospy.Rate(60)
        while np.linalg.norm(error) > dist_tol and not rospy.is_shutdown():
            if not self.new_image_arrived():
                continue

            # Continue if no corners detected
            marker_corners = self._get_detected_corners()
            if marker_corners is None:
                continue

            # Don't move if the target hasn't been set
            if not self._visual_servo._target_set:
                continue

            # Get control law velocity and transform to body frame, then send to robot
            servo_vel = self._visual_servo.get_next_vel(corners=marker_corners)
            self._command_velocity(servo_vel)
            r.sleep()
Пример #13
0
class IbvsEih(object):
    """
    Performs eye in hand (eih) image based visual servoing (ibvs). 
    """
    def __init__(self):
        # Baxter specific code. You don't need this if you're using another robot.
        # Sets the arm that is used for the servoing
        limb='left'
        self._baxter = BaxterVS(limb)

        self.gripper = baxter_interface.Gripper(limb)
        self.gripper.calibrate()

        self.golf_ball_x = 0.85                        # x     = front back
        self.golf_ball_y = 0.10                        # y     = left right
        self.golf_ball_z = 0.0                        # z     = up down
        self.roll        = -1.0 * math.pi              # roll  = horizontal
        self.pitch       = 0.0 * math.pi               # pitch = vertical
        self.yaw         = 0.0 * math.pi               # yaw   = rotation

        self.pose = (self.golf_ball_x,
                        self.golf_ball_y,
                        self.golf_ball_z,
                        self.roll,
                        self.pitch,
                        self.yaw)
        print 'ready to move'
        self.baxter_ik_move(limb, self.pose)
        # self.golf_ball_x = self.golf_ball_x + 0.010
        # # self.golf_ball_y = self.golf_ball_y - 0.10
        # # self.golf_ball_z = self.golf_ball_z + 0.10  
        # self.pitch = self.pitch - 0.2 * math.pi 
        # self.pose = (self.golf_ball_x,
        #                 self.golf_ball_y,
        #                 self.golf_ball_z,
        #                 self.roll,
        #                 self.pitch,
        #                 self.yaw)
        # print 'ready to move'
        # self.baxter_ik_move(limb, self.pose)          
        # AprilTag specific code. You don't need this if you're using another tracking system.
        # Initializes the marker that the arm should track
        target_marker = 0
        # self._apriltag_client = AprilTagClient(target_marker)

        self._visual_servo = VisualServoing(ibvs=True)

        # camera parameters (NB. other parameters in open_camera)
        self.cam_calib    = 0.0025                     # meters per pixel at 1 meter
        self.cam_x_offset = -0.01                   # camera gripper offset
        self.cam_y_offset = -0.035
        self.width        = 960                       # Camera resolution
        self.height       = 600

        # callback image
        self.cv_image = cv.CreateImage((self.width, self.height), 8, 3)

        # create image publisher to head monitor
        self.pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
        self.subscribe_to_camera(limb)
    
    def new_image_arrived(self):
        """
        Boolean to test if a new image has arrived.
        """
        print "waiting for new image"
        if self._apriltag_client.corners is not None:
            self._apriltag_client.corners = None
            return True
        return False

    # def _get_detected_corners(self):
    #     """
    #     Returns the most recently detected corners in the image.
    #     """
    #     # This method currently uses AprilTag detections, so replace with
    #     # your own method if otherwise.
    #     # print "looking for corners"
    #     return self._apriltag_client.corners
    
    def _command_velocity(self,cam_vel,error_norm):
        """
        Move the camera at the specified v and omega in vel (6x1 vector).
        """
        # This method currently commands the Baxter robot from Rethink Robotics.
        #Replace with your own method if using another manipulator.
        
        hand_vel_in_base = self._baxter.cam_to_body(cam_vel)
        # print 'baxter_vel: ', baxter_vel
        self._baxter.set_hand_vel(hand_vel_in_base, error_norm)
    
    def set_target(self,final_camera_depth,desired_corners):
        """
        Sets the final camera depth (Z) and desired position for the tracked features
        at the goal position.
        """


        Z = final_camera_depth
        ideal_cam_pose = np.array([0,0,Z])  
        # print 'ideal_cam_pose: ', ideal_cam_pose
        self._visual_servo.set_target(ideal_cam_pose,None,desired_corners)
    
    def move_to_position(self,final_camera_depth,desiredImg, desiredkps, desiredkps_npa, desiredFeatures,dist_tol):
        """
        Runs one instance of the visual servoing control law. Call when a new
        image has arrived.
        """
        
        # self.set_target(final_camera_depth,desired_corners)
        # r = rospy.Rate(60)
        error_norm = 1000
        hand_pose = baxter.get_arm_pose('left')
        count = 1
        while error_norm>dist_tol and not rospy.is_shutdown() and not hand_pose.position.z < -0.371:
            # if not self.new_image_arrived():
            #     # print " no new_image_arrived"
            #     continue
            
            # Continue if no corners detected
            # marker_corners = self._get_detected_corners()
            # img_array = cv2array(self.cv_image)
            # print self.cv_image.shape
            # cv.WaitKey(1000)
            currentI = self.cv_image
            (detectedkps, detectedkps_npa, detectedFeatures) = self.detectAndDescribe(currentI)

            img=cv2.drawKeypoints(currentI,detectedkps)
            
            # plt.figure(1);
            # plt.show(plt.imshow(img))

            # plt.figure(1);
            # plt.show(plt.imshow(img))

            ratio=0.75
            reprojThresh=4.0
            # print 'desiredkps_npa: ', desiredkps_npa
            # print 'desiredFeatures: ', desiredFeatures
            (matches, H, status) = self.matchKeypoints(desiredkps_npa, detectedkps_npa, desiredFeatures, detectedFeatures, ratio, reprojThresh)
            selected_matches_id = np.where(status != 0)[0]
            # if matches is None:
            #     # print "no marker corners"
            #     continue
            if len(selected_matches_id) < 4:
                print "no enough matching"
                continue

            # selected_matches_id = selected_matches_id[0:3]

            print 'mathces:', len(selected_matches_id)
            # print 'desiredkps_npa: ', desiredkps_npa
            # print 'status: ', status

            # cv2.waitKey(0)

            # print desiredkps_npa.shape
            # print detectedkps_npa.shape
            # print detectedkps_npa
            # print 'mathces:', matches
            # print 'mathces[1]:', matches[1][0]

            desired_corners = np.zeros(len(selected_matches_id)*2)
            detected_corners = np.zeros(len(selected_matches_id)*2)
            # selected_matches = [(0,0),(0,0),(0,0)]

            for i in range(0,len(selected_matches_id)):
                desired_m = desiredkps_npa[matches[selected_matches_id[i]][1]]
                desired_corners[2*i] = desired_m[0]
                desired_corners[2*i+1] = desired_m[1]
                detected_m = detectedkps_npa[matches[selected_matches_id[i]][0]]
                detected_corners[2*i] = detected_m[0]
                detected_corners[2*i+1] = detected_m[1]
                # selected_matches[i] = matches[selected_matches_id[i]] 

            # print 'desired_corners: ', desired_corners
            # print 'detected_corners: ', detected_corners
            # vis = self.drawMatches(desiredImg, currentI, desiredkps_npa, detectedkps_npa, matches, status)
            # print 'selected_matches: ', selected_matches

            self.set_target(final_camera_depth,desired_corners)



            # Don't move if the target hasn't been set
            if not self._visual_servo._target_set:
                print "the target hasn't been set"
                continue
            
            # Get control law velocity and transform to body frame, then send to robot
            cam_vel, error = self._visual_servo.get_next_vel(corners = detected_corners)
            error_norm = np.linalg.norm(error)/(len(detected_corners)/2)
            self._command_velocity(cam_vel, error_norm)
            # print 'servo_vel: ', servo_vel
            # print 'before sleep'
            # print 'error: ', np.linalg.norm(error)
            # print rospy.is_shutdown()
            # r.sleep()
            # cv.WaitKey(1000)
            vis = self.drawMatches(desiredImg, currentI, desiredkps_npa, detectedkps_npa, matches, status)
            # vis = -self.drawMatches(desiredImg, currentI, desiredkps_npa, detectedkps_npa, selected_matches, np.ones(3))
            cv2.imshow("Keypoint Matches", vis)
            filename = '/home/pracsys/shaojun/visual_servoing_ws/'+str(count) +'.jpg'
            cv2.imwrite(filename, vis)
            # cv2.waitKey(0)

            msg = cv_bridge.CvBridge().cv2_to_imgmsg(vis, encoding="bgr8")
            self.pub.publish(msg)

            rospy.sleep(0.8)
            print 'count: ', count
            count = count + 1




        hand_pose = baxter.get_arm_pose('left')
        print 'hand_pose:', hand_pose

        self.golf_ball_x = hand_pose.position.x + 0.05                     # x     = front back
        self.golf_ball_y = hand_pose.position.y                       # y     = left right
        self.golf_ball_z = hand_pose.position.z                        # z     = up down
        self.orientation = hand_pose.orientation

        self.pose = (self.golf_ball_x,
                        self.golf_ball_y,
                        self.golf_ball_z,
                        self.orientation.x,
                        self.orientation.y,
                        self.orientation.z,
                        self.orientation.w)

        print

        print 'ready to move'
        self.baxter_ik_move('left', self.pose)

        time.sleep(1)

        self.gripper.close()
        time.sleep(1)

        hand_pose = baxter.get_arm_pose('left')
        self.golf_ball_x = hand_pose.position.x                       # x     = front back
        self.golf_ball_y = hand_pose.position.y                       # y     = left right
        self.golf_ball_z = hand_pose.position.z+0.03                       # z     = up down
        self.orientation = hand_pose.orientation

        self.pose = (self.golf_ball_x,
                        self.golf_ball_y,
                        self.golf_ball_z,
                        self.orientation.x,
                        self.orientation.y,
                        self.orientation.z,
                        self.orientation.w)

        print 'ready to move'
        self.baxter_ik_move('left', self.pose)

        hand_pose = baxter.get_arm_pose('left')
        self.golf_ball_x = hand_pose.position.x-0.15                       # x     = front back
        self.golf_ball_y = hand_pose.position.y                       # y     = left right
        self.golf_ball_z = hand_pose.position.z                       # z     = up down
        self.orientation = hand_pose.orientation

        self.pose = (self.golf_ball_x,
                        self.golf_ball_y,
                        self.golf_ball_z,
                        self.orientation.x,
                        self.orientation.y,
                        self.orientation.z,
                        self.orientation.w)

        print

        print 'ready to move'
        self.baxter_ik_move('left', self.pose)




    def baxter_ik_move(self, limb, rpy_pose):
        quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base")

        node = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
        ik_service = rospy.ServiceProxy(node, SolvePositionIK)
        ik_request = SolvePositionIKRequest()
        hdr = Header(stamp=rospy.Time.now(), frame_id="base")

        ik_request.pose_stamp.append(quaternion_pose)
        try:
            rospy.wait_for_service(node, 5.0)
            ik_response = ik_service(ik_request)
        except (rospy.ServiceException, rospy.ROSException), error_message:
            rospy.logerr("Service request failed: %r" % (error_message,))
            sys.exit("ERROR - baxter_ik_move - Failed to append pose")

        if ik_response.isValid[0]:
            print("PASS: Valid joint configuration found")
            # convert response to joint position control dictionary
            limb_joints = dict(zip(ik_response.joints[0].name, ik_response.joints[0].position))
            # move limb
            # if self.limb == limb:
            limb_interface = baxter_interface.Limb(limb)
            limb_interface.move_to_joint_positions(limb_joints)
            print 'moving'
            # else:
                # self.other_limb_interface.move_to_joint_positions(limb_joints)
        else:
            # display invalid move message on head display
            # self.splash_screen("Invalid", "move")
            # little point in continuing so exit with error message
            print "requested move =", rpy_pose
            sys.exit("ERROR - baxter_ik_move - No valid joint configuration found")

        # if self.limb == limb:               # if working arm
        quaternion_pose = limb_interface.endpoint_pose()
        position        = quaternion_pose['position']

        # if working arm remember actual (x,y) position achieved
        self.pose = [position[0], position[1],                                \
                     self.pose[2], self.pose[3], self.pose[4], self.pose[5]]