text:
#coding=utf-8
from __future__ import print_function
import cv2 as cv
import numpy as np
from matplotlib import pylab as plt
import argparse
import glob
from math import pi
import urx
import logging
import time
import sys
import math3d as m3d
def move_to_dui1():
    rob = urx.Robot("192.168.80.2")
    rob.set_tcp((0,0,0,0,0,0))
    rob.set_payload(0.5, (0,0,0))
    v = 0.02
    a = 0.1
    #dui1 pose start
    pose = [-0.015371047712964686, 0.16291898650384315, 0.3583823679602556, 0.729218924720313, 1.756045637777791, 1.72787322187088]
    rob.movel(pose, acc=a, vel=v)
    rob.close()

def robot_move_OnePose():
    # to move up
    print("moving to a known pose")
    rob = urx.Robot("192.168.80.2")
    rob.set_tcp((0,0,0,0,0,0))
    rob.set_payload(0.5, (0,0,0))

    v = 0.01
    a = 0.1

    pose = rob.getl()
    print("robot tcp is at: ", pose)    #   in the world coordinate position and rotation

    # move in z direction up for 0.02
    # print("moving in z")

    pose[0] = 0.1
    pose[1] = 0.1
    pose[2] = 1.0
    rob.movel(pose, acc=a, vel=v)
    rob.close()

def rotate_forward():
    print("rotate_forward")
    rob = urx.Robot("192.168.80.2")
    rob.set_tcp((0,0,0,0,0,0))
    rob.set_payload(0.5, (0,0,0))

    v = 0.02
    a = 0.1

    pose = rob.getl()
    print("robot tcp is at: ", pose)    #   in the world coordinate position and rotation

    # move in x direction  for 0.02
    # print("moving in x")
    pose[4] += pi/90
    rob.movel(pose, acc=a, vel=v)
    rob.close()

def rotate_left():
    print("rotate_left")

def rotate_up():
    print("rotate_up")

def robot_move_right1(baselength):
    # to move right l unit:mm
    print("moving right")
    rob = urx.Robot("192.168.80.2")
    rob.set_tcp((0,0,0,0,0,0))
    rob.set_payload(0.5, (0,0,0))

    v = 0.02
    a = 0.1

    pose = rob.getl()
    print("robot tcp is at: ", pose)    #   in the world coordinate position and rotation

    # move in x direction  for 0.02
    # print("moving in x")
    pose[0] += baselength
    rob.movel(pose, acc=a, vel=v)
    rob.close()
def robot_move_forward1():
    # to move forward
    print("moving forward")
    rob = urx.Robot("192.168.80.2")
    rob.set_tcp((0,0,0,0,0,0))
    rob.set_payload(0.5, (0,0,0))

    v = 0.02
    a = 0.1

    pose = rob.getl()
    print("robot tcp is at: ", pose)    #   in the world coordinate position and rotation

    # move in y direction  for 0.02
    # print("moving in y")
    pose[1] -= 0.02
    rob.movel(pose, acc=a, vel=v)
    rob.close()

# to find the known object and print the corner point location
def match(c):
    parser = argparse.ArgumentParser(description='Code for Feature Matching with FLANN tutorial.')
    parser.add_argument('--input1', help='Path to input image 1.', default='rec_small1.jpg')
    parser.add_argument('--input2', help='Path to input image 2.', default='frame' + str(c) + '.jpg')
    args = parser.parse_args()
    img_object = cv.imread(args.input1, cv.IMREAD_GRAYSCALE)
    img_scene = cv.imread(args.input2, cv.IMREAD_GRAYSCALE)
    if img_object is None or img_scene is None:
        print('Could not open or find the images!')
        exit(0)
    #-- Step 1: Detect the keypoints using SURF Detector, compute the descriptors
    minHessian = 400
    detector = cv.xfeatures2d_SURF.create(hessianThreshold=minHessian)
    keypoints_obj, descriptors_obj = detector.detectAndCompute(img_object, None)
    keypoints_scene, descriptors_scene = detector.detectAndCompute(img_scene, None)
    #-- Step 2: Matching descriptor vectors with a FLANN based matcher
    # Since SURF is a floating-point descriptor NORM_L2 is used
    matcher = cv.DescriptorMatcher_create(cv.DescriptorMatcher_FLANNBASED)
    knn_matches = matcher.knnMatch(descriptors_obj, descriptors_scene, 2)
    #-- Filter matches using the Lowe's ratio test
    ratio_thresh = 0.75
    good_matches = []
    for m,n in knn_matches:
        if m.distance < ratio_thresh * n.distance:
            good_matches.append(m)
    #-- Draw matches
    img_matches = np.empty((max(img_object.shape[0], img_scene.shape[0]), img_object.shape[1]+img_scene.shape[1], 3), dtype=np.uint8)
    cv.drawMatches(img_object, keypoints_obj, img_scene, keypoints_scene, good_matches, img_matches, flags=cv.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS)
    #-- Localize the object
    obj = np.empty((len(good_matches),2), dtype=np.float32)
    scene = np.empty((len(good_matches),2), dtype=np.float32)
    for i in range(len(good_matches)):
        #-- Get the keypoints from the good matches
        obj[i,0] = keypoints_obj[good_matches[i].queryIdx].pt[0]
        obj[i,1] = keypoints_obj[good_matches[i].queryIdx].pt[1]
        scene[i,0] = keypoints_scene[good_matches[i].trainIdx].pt[0]
        scene[i,1] = keypoints_scene[good_matches[i].trainIdx].pt[1]
    H, _ =  cv.findHomography(obj, scene, cv.RANSAC)
    #-- Get the corners from the image_1 ( the object to be "detected" )
    obj_corners = np.empty((4,1,2), dtype=np.float32)
    obj_corners[0,0,0] = 0
    obj_corners[0,0,1] = 0
    obj_corners[1,0,0] = img_object.shape[1]
    obj_corners[1,0,1] = 0
    obj_corners[2,0,0] = img_object.shape[1]
    obj_corners[2,0,1] = img_object.shape[0]
    obj_corners[3,0,0] = 0
    obj_corners[3,0,1] = img_object.shape[0]
    scene_corners = cv.perspectiveTransform(obj_corners, H)
    #-- Draw lines between the corners (the mapped object in the scene - image_2 )
    cv.line(img_matches, (int(scene_corners[0,0,0] + img_object.shape[1]), int(scene_corners[0,0,1])),\
        (int(scene_corners[1,0,0] + img_object.shape[1]), int(scene_corners[1,0,1])), (0,255,0), 4)
    cv.line(img_matches, (int(scene_corners[1,0,0] + img_object.shape[1]), int(scene_corners[1,0,1])),\
        (int(scene_corners[2,0,0] + img_object.shape[1]), int(scene_corners[2,0,1])), (0,255,0), 4)
    cv.line(img_matches, (int(scene_corners[2,0,0] + img_object.shape[1]), int(scene_corners[2,0,1])),\
        (int(scene_corners[3,0,0] + img_object.shape[1]), int(scene_corners[3,0,1])), (0,255,0), 4)
    cv.line(img_matches, (int(scene_corners[3,0,0] + img_object.shape[1]), int(scene_corners[3,0,1])),\
        (int(scene_corners[0,0,0] + img_object.shape[1]), int(scene_corners[0,0,1])), (0,255,0), 4)
    #-- Show detected matches
    cv.imshow('Good Matches & Object detection', img_matches)
    cv.imwrite('Good Matches&Object detection'+str(c) + '.jpg',img_matches)
    # print(int(scene_corners[0,0,0]),img_object.shape[1],int(scene_corners[0,0,1]))
    # print('Good Matches&Object detection1'+str(c) + '.jpg')
    center_point0=(int(scene_corners[0,0,0] + img_object.shape[1])+int(scene_corners[2,0,0] + img_object.shape[1]))/2
    center_point1=(int(scene_corners[0,0,1])+int(scene_corners[2,0,1]))/2
    center_point=[center_point0,center_point1]
    print(center_point)
    return(center_point)

def match1():
    parser = argparse.ArgumentParser(description='Code for Feature Matching with FLANN tutorial.')
    parser.add_argument('--input1', help='Path to input image 1.', default='rec_small1.jpg')
    parser.add_argument('--input2', help='Path to input image 2.', default='facing1.jpg')
    args = parser.parse_args()
    img_object = cv.imread(args.input1, cv.IMREAD_GRAYSCALE)
    img_scene = cv.imread(args.input2, cv.IMREAD_GRAYSCALE)
    if img_object is None or img_scene is None:
        print('Could not open or find the images!')
        exit(0)
    #-- Step 1: Detect the keypoints using SURF Detector, compute the descriptors
    minHessian = 400
    detector = cv.xfeatures2d_SURF.create(hessianThreshold=minHessian)
    keypoints_obj, descriptors_obj = detector.detectAndCompute(img_object, None)
    keypoints_scene, descriptors_scene = detector.detectAndCompute(img_scene, None)
    #-- Step 2: Matching descriptor vectors with a FLANN based matcher
    # Since SURF is a floating-point descriptor NORM_L2 is used
    matcher = cv.DescriptorMatcher_create(cv.DescriptorMatcher_FLANNBASED)
    knn_matches = matcher.knnMatch(descriptors_obj, descriptors_scene, 2)
    #-- Filter matches using the Lowe's ratio test
    ratio_thresh = 0.75
    good_matches = []
    for m,n in knn_matches:
        if m.distance < ratio_thresh * n.distance:
            good_matches.append(m)
    #-- Draw matches
    img_matches = np.empty((max(img_object.shape[0], img_scene.shape[0]), img_object.shape[1]+img_scene.shape[1], 3), dtype=np.uint8)
    cv.drawMatches(img_object, keypoints_obj, img_scene, keypoints_scene, good_matches, img_matches, flags=cv.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS)
    #-- Localize the object
    obj = np.empty((len(good_matches),2), dtype=np.float32)
    scene = np.empty((len(good_matches),2), dtype=np.float32)
    for i in range(len(good_matches)):
        #-- Get the keypoints from the good matches
        obj[i,0] = keypoints_obj[good_matches[i].queryIdx].pt[0]
        obj[i,1] = keypoints_obj[good_matches[i].queryIdx].pt[1]
        scene[i,0] = keypoints_scene[good_matches[i].trainIdx].pt[0]
        scene[i,1] = keypoints_scene[good_matches[i].trainIdx].pt[1]
    H, _ =  cv.findHomography(obj, scene, cv.RANSAC)
    #-- Get the corners from the image_1 ( the object to be "detected" )
    obj_corners = np.empty((4,1,2), dtype=np.float32)
    obj_corners[0,0,0] = 0
    obj_corners[0,0,1] = 0
    obj_corners[1,0,0] = img_object.shape[1]
    obj_corners[1,0,1] = 0
    obj_corners[2,0,0] = img_object.shape[1]
    obj_corners[2,0,1] = img_object.shape[0]
    obj_corners[3,0,0] = 0
    obj_corners[3,0,1] = img_object.shape[0]
    scene_corners = cv.perspectiveTransform(obj_corners, H)
    #-- Draw lines between the corners (the mapped object in the scene - image_2 )
    cv.line(img_matches, (int(scene_corners[0,0,0] + img_object.shape[1]), int(scene_corners[0,0,1])),\
        (int(scene_corners[1,0,0] + img_object.shape[1]), int(scene_corners[1,0,1])), (0,255,0), 4)
    cv.line(img_matches, (int(scene_corners[1,0,0] + img_object.shape[1]), int(scene_corners[1,0,1])),\
        (int(scene_corners[2,0,0] + img_object.shape[1]), int(scene_corners[2,0,1])), (0,255,0), 4)
    cv.line(img_matches, (int(scene_corners[2,0,0] + img_object.shape[1]), int(scene_corners[2,0,1])),\
        (int(scene_corners[3,0,0] + img_object.shape[1]), int(scene_corners[3,0,1])), (0,255,0), 4)
    cv.line(img_matches, (int(scene_corners[3,0,0] + img_object.shape[1]), int(scene_corners[3,0,1])),\
        (int(scene_corners[0,0,0] + img_object.shape[1]), int(scene_corners[0,0,1])), (0,255,0), 4)
    #-- Show detected matches
    cv.imshow('Good Matches & Object detection', img_matches)
    # cv.imwrite('Good Matches&Object detection1.jpg',img_matches)
    # cv.imshow('Good Matches&Object detection1.jpg',img_matches)
    # print(int(scene_corners[0,0,0]),img_object.shape[1],int(scene_corners[0,0,1]))
    # print('Good Matches&Object detection1'+str(c) + '.jpg')
    center_point0=(int(scene_corners[0,0,0] + img_object.shape[1])+int(scene_corners[2,0,0] + img_object.shape[1]))/2
    center_point1=(int(scene_corners[0,0,1])+int(scene_corners[2,0,1]))/2
    center_point=[center_point0,center_point1]
    # return(center_point)
    print(center_point)
    while(True):
    # Press Q on keyboard to  exit
        if cv.waitKey(25) & 0xFF == ord('q'):
            break

def move_point():
    # to move up
    print("moving to the point")
    rob = urx.Robot("192.168.80.2")
    rob.set_tcp((0,0,0,0,0,0))
    rob.set_payload(0.5, (0,0,0))
    v = 0.02
    a = 0.1
    #close1-6
    # pose =  [0.11331817392983798, 0.2801514404360457, 0.40716238831066137, 0.7288518769893056, 1.75720949766542, 1.7288762714958366]
    # rob.movel(pose, acc=a, vel=v)
    rob.close()

def robot_move_task():
    # robot_move_task_from_ReferencePosition
    # to move as the task asked
    print("moving to complate the task")
    rob = urx.Robot("192.168.80.2")
    rob.set_tcp((0,0,0,0,0,0))
    rob.set_payload(0.5, (0,0,0))

    l = 0.02
    v = 0.02
    a = 0.1

    pose = rob.getl()
    print("robot tcp is at: ", pose)    #   in the world coordinate position and rotation

    #dui1 pose start
    # pose = [-0.015371047712964686, 0.16291898650384315, 0.3583823679602556, 0.729218924720313, 1.756045637777791, 1.72787322187088]
    # rob.movel(pose, acc=a, vel=v)
    # step1
    pose = [0.09843593470393579, 0.26510182111053154, 0.4028641488364185, 0.7293840999220157, 1.7575432907350066, 1.7280083427019985]
    rob.movel(pose, acc=a, vel=v)
    # step2
    pose =  [0.11349549374315637, 0.2802989236507926, 0.40720806884446564, 0.7290357525244547, 1.7569778326154977, 1.7287120933853668]
    rob.movel(pose, acc=a, vel=v)
    # step3
    pose =  [0.11460174179046292, 0.28153593539694216, 0.4074298765968609, 0.7295337903752471, 1.756310919064192, 1.7283559296692148]
    rob.movel(pose, acc=a, vel=v)
    pose =  [0.12923785534015672, 0.2671670032288611, 0.40747408164237053, 0.7295000886040007, 1.7572500353651812, 1.7283636977205024]
    rob.movel(pose, acc=a, vel=v)
    pose =  [0.11078068722321228, 0.25527572034962204, 0.40764048056389807, 0.7298588022893313, 1.7555891125498237, 1.7284234936835599]
    rob.movel(pose, acc=a, vel=v)
    pose =  [0.07648318951482813, 0.22131664005025137, 0.4077336729334317, 0.7290399452789309, 1.7566098675466841, 1.7290427412939582]
    rob.movel(pose, acc=a, vel=v)
    time.sleep(2)
    #in 1-4
    pose =  [0.1096277417105041, 0.2782563939867772, 0.40945510584463063, 0.7304462462673038, 1.7569070677822813, 1.7268994057911478]
    rob.movel(pose, acc=a, vel=v)
    pose =  [0.11813817891258888, 0.2856367025277646, 0.41328282986770337, 0.7293913095346095, 1.757686739360187, 1.7281365277259495]
    rob.movel(pose, acc=a, vel=v)
    pose =  [0.11939772214092868, 0.2885459066988721, 0.411258361377801, 0.729510577813205, 1.7552934806593876, 1.7289513919241533]
    rob.movel(pose, acc=a, vel=v)
    pose =  [0.12358349828409654, 0.29316420912719965, 0.410826266327851, 0.7290838685610902, 1.757399470553258, 1.7285771052298267]
    rob.movel(pose, acc=a, vel=v)
    pose =  [0.13147317314094595, 0.3011585845734863, 0.41078134124622057, 0.728876103815376, 1.7572176942540814, 1.7289702636115485]
    rob.movel(pose, acc=a, vel=v)
    pose =  [0.1405024103151181, 0.310255323239094, 0.4110833042894985, 0.7287406831381755, 1.7573406460681256, 1.7291010739354038]
    rob.movel(pose, acc=a, vel=v)
    pose =  [0.14239719241844506, 0.31252705462501135, 0.41209069321252606, 0.7291499575488868, 1.7628837802632797, 1.7265063352354237]
    rob.movel(pose, acc=a, vel=v)
    time.sleep(5)
    #out 2-6
    pose =  [0.1334431445545815, 0.3026290175057595, 0.4118846240821475, 0.730741243829314, 1.760253414711346, 1.7255141352815433]
    rob.movel(pose, acc=a, vel=v)
    pose =  [0.12437409880672887, 0.29382238237474473, 0.41159802946857316, 0.7305493146697993, 1.7623292137236457, 1.724746836319876]
    rob.movel(pose, acc=a, vel=v)
    pose =  [0.11798124860681335, 0.28735259472234725, 0.4115068046183376, 0.7308702421744679, 1.7625799214084237, 1.7244085447547393]
    rob.movel(pose, acc=a, vel=v)
    pose =  [0.08526943020955498, 0.2547964393826721, 0.4116081134616931, 0.7302656953431732, 1.761960373435015, 1.7250979247124252]
    rob.movel(pose, acc=a, vel=v)
    pose =  [0.0592818549986954, 0.22880215454237002, 0.4115284040360025, 0.7307364002519257, 1.7630925285669465, 1.7241497309932778]
    rob.movel(pose, acc=a, vel=v)
    #close1-6
    pose =  [0.03907648121863697, 0.24824147478697123, 0.4113091730563136, 0.7301457328402742, 1.7633817788209647, 1.724228231133081]
    rob.movel(pose, acc=a, vel=v)
    pose =  [0.020770591111322328, 0.2668176069070978, 0.4114559310892153, 0.730043416937704, 1.7621468652746528, 1.7246688495323277]
    rob.movel(pose, acc=a, vel=v)
    pose =  [0.007834567566814908, 0.2799198500316586, 0.41113026596905333, 0.7296730780348917, 1.763338816584342, 1.7238073259520172]
    rob.movel(pose, acc=a, vel=v)
    pose =  [-0.002584685455172076, 0.29037234929837574, 0.41119475375580194, 0.7297502879408158, 1.7632873208189492, 1.7238532499992436]
    rob.movel(pose, acc=a, vel=v)
    pose =  [0.04506127505101218, 0.33840389742311, 0.41116776085679263, 0.7291835964187542, 1.7638554918217029, 1.724986842882753]
    rob.movel(pose, acc=a, vel=v)
    pose =  [0.09526823207181682, 0.263550292508512, 0.41155533470618194, 0.729797114405243, 1.7631994432180738, 1.7253466398567225]
    rob.movel(pose, acc=a, vel=v)
    time.sleep(2)
    pose =  [0.11489694220364684, 0.2821064908742386, 0.406623292470202, 0.7287839192148127, 1.7599187617629883, 1.7278966961919806]
    rob.movel(pose, acc=a, vel=v)
    #close OK
    time.sleep(2)
    #return to the original point
    pose =  [-0.026091850507074233, 0.13637172277017537, 0.34192834637749725, 0.7168010678810264, 1.9995058927833398, 1.4523042091915035]
    rob.movel(pose, acc=a, vel=v)

    # rob.movel(pose, acc=a, vel=v)
    rob.close()

def Updown2dui1():
    cap = cv.VideoCapture(0)
    c = 0
    fps = 10   # FPS of the video
    # rval=cap.isOpened()
    fourcc = cv.VideoWriter_fourcc(*'MJPG')
    # the last parameter is the size
    # videoWriter = cv.VideoWriter('saveVideo.avi', fourcc, fps, (640, 480))
    # Read until video is completed
    # ideal_center_point=[600,242]
    ideal_center_point=[450,211]
    delta_point=[0,0]
    while(cap.isOpened()):
        c = c + 1
        # Capture frame-by-frame
        ret, frame = cap.read()
        if ret == True:
            # Display the resulting frame
            cv.imshow('Frame', frame)
            cv.imwrite('frame'+str(c) + '.jpg',frame)   # save as pictures
            # frame1 = frame

            # print("find the dui1 pose")
            real_center_point=match(c)
            delta_point[0]=real_center_point[0]-ideal_center_point[0]
            delta_point[1]=real_center_point[1]-ideal_center_point[1]
            print(delta_point[0])

            if((delta_point[1]>15)|(delta_point[1]<-15)):
                if (delta_point[1])>0:
                    robot_move_down()
                if (delta_point[1])<0:
                    robot_move_up()
            else:
                print("Up and down is OK")
                break
            # Press Q on keyboard to  exit
            if cv.waitKey(25) & 0xFF == ord('q'):
                break
        # Break the loop
        else:
            break

    # When everything done, release the video capture object
    cap.release()

    # Closes all the frames
    cv.destroyAllWindows()

def LeftRight2dui1():
    cap = cv.VideoCapture(0)
    c = 0
    fps = 10   # FPS of the video
    # rval=cap.isOpened()
    fourcc = cv.VideoWriter_fourcc(*'MJPG')
    # the last parameter is the size
    # videoWriter = cv.VideoWriter('saveVideo.avi', fourcc, fps, (640, 480))
    # Read until video is completed
    # ideal_center_point=[600,242]
    ideal_center_point=[450,211]
    delta_point=[0,0]
    while(cap.isOpened()):
        c = c + 1
        # Capture frame-by-frame
        ret, frame = cap.read()
        if ret == True:
            # Display the resulting frame
            cv.imshow('Frame', frame)
            cv.imwrite('frame'+str(c) + '.jpg',frame)   # save as pictures
            # frame1 = frame

            # print("find the dui1 pose")
            real_center_point=match(c)
            delta_point[0]=real_center_point[0]-ideal_center_point[0]
            delta_point[1]=real_center_point[1]-ideal_center_point[1]
            print(delta_point[0])

            if(((delta_point[0]>15))|((delta_point[0])<-15)):
                if delta_point[0]>0:
                    robot_move_right()
                if delta_point[0]<0:
                    robot_move_left()
            else:
                print("Left and right is OK")
                break
            # Press Q on keyboard to  exit
            if cv.waitKey(25) & 0xFF == ord('q'):
                break
        # Break the loop
        else:
            break

    # When everything done, release the video capture object
    cap.release()

    # Closes all the frames
    cv.destroyAllWindows()

def ForwardBack2dui1():
    cap = cv.VideoCapture(0)
    c = 0
    fps = 10   # FPS of the video
    # rval=cap.isOpened()
    fourcc = cv.VideoWriter_fourcc(*'MJPG')
    # the last parameter is the size
    # videoWriter = cv.VideoWriter('saveVideo.avi', fourcc, fps, (640, 480))
    # Read until video is completed
    ideal_center_point=[600,242]
    delta_point=[0,0]
    while(cap.isOpened()):
        c = c + 1
        # Capture frame-by-frame
        ret, frame = cap.read()
        if ret == True:
            # Display the resulting frame
            cv.imshow('Frame', frame)
            cv.imwrite('frame'+str(c) + '.jpg',frame)   # save as pictures
            # frame1 = frame

            # print("find the dui1 pose")
            real_center_point=match(c)
            delta_point[0]=real_center_point[0]-ideal_center_point[0]
            delta_point[1]=real_center_point[1]-ideal_center_point[1]
            print(delta_point[0])

            if((delta_point[1]>15)|(delta_point[1]<-15)):
                if (delta_point[1])>0:
                    robot_move_down()
                if (delta_point[1])<0:
                    robot_move_up()
            else:
                print("Up and down is OK")
                break
            # Press Q on keyboard to  exit
            if cv.waitKey(25) & 0xFF == ord('q'):
                break
        # Break the loop
        else:
            break

    # When everything done, release the video capture object
    cap.release()

    # Closes all the frames
    cv.destroyAllWindows()

# pose_dui1 =  [-0.015379972627099047, 0.16278659829816503, 0.35778023133243914, 0.7293855033267473, 1.7571449945285949, 1.7263708455148004]

# if __name__ == '__main__':
    # match1()
    # move_to_dui1()
    # robot_move_task()
    # print_robot_tcp()
    # robot_move_up()
    # robot_move_forward1()
    # rotate_forward()
    # get_Transformation()

    # 此处输出的Orientation为RxRyRz旋转矩阵相乘的出来的结果
    # pose = [-0.015379972627099047, 0.16278659829816503, 0.35778023133243914, 0.7293855033267473, 1.7571449945285949, 1.7263708455148004]
    # pose = [1.57, 0, 0, 1.57, 0, 0]
    # print(m3d.Transform())

    # robot_move_right1(0.01)   # 最多只能移动50cm
    # delta_robot_tcp1 = print_delta_robot_tcp()
    # delta_robot_tcp1 =  [-8.38816606e-05, 4.68648427e-05, 4.68079196e-04, -2.88025478e-02, -7.31398710e-02, 5.52882549e-02]
    # test_delta_robot_tcp(delta_robot_tcp1)  # test_delta_robot_tcp() function OK

# Teach and peg in the hole_OK
    # Updown2dui1()
    # time.sleep(3)
    # LeftRight2dui1()
    # time.sleep(3)
    # robot_move_task()


text:
import tensorflow as tf
import SimpleITK as sitk
import numpy as np
import os
import json
import argparse
import logging
from utils.sitk_utils import resample_to_spacing
from utils.utils import start_logger
from batchgenerators.augmentations.spatial_transformations \
    import augment_spatial
from batchgenerators.augmentations.noise_augmentations \
    import augment_gaussian_noise

def _int64_feature(value):
    return tf.train.Feature(int64_list=tf.train.Int64List(value=[value]))

def _bytes_feature(value):
    return tf.train.Feature(bytes_list=tf.train.BytesList(value=[value]))

def read_json(fpath):
    with open(fpath, 'r') as fp:
        return(json.load(fp))

def write_json(f, fpath):
    with open(fpath, 'w') as fp:
        return(json.dump(f, fp))

def transforms(volume, segmentation, patch_size_, fraction_, spacing_,
               variance_):
    # BEWARE SPACING: dset.spacing = (x,y,z) but volume is shaped (z,x,y))
    volume, augmentation, rval = augment_spatial(
                                    volume,
                                    segmentation,
                                    patch_size=patch_size_,
                                    patch_center_dist_from_border=np.array(
                                        patch_size_) // 2,
                                    border_mode_data='constant',
                                    border_mode_seg='constant',
                                    border_cval_data=np.min(volume),
                                    border_cval_seg=0,
                                    alpha=(0, 750),
                                    sigma=(10, 13),
                                    scale=(0.8, 1.2),
                                    do_elastic_deform=True,
                                    do_scale=True,
                                    do_rotation=True,
                                    angle_x=(0, 2*np.pi),
                                    angle_y=(0, 0),
                                    angle_z=(0, 0),
                                    fraction=fraction_,
                                    spacing=spacing_)
    if np.any(variance_ != 0):
        volume = augment_gaussian_noise(volume, noise_variance=variance_)
    return volume, augmentation, rval

class TFRdataset():
    def __init__(self, fpath):
        self.fpath = fpath
        self.dset = read_json(os.path.join(self.fpath, 'dataset.json'))
        self.num_classes = len(self.dset['labels'])
        self.num_modalities = len(self.dset['modality'])
        self.batch_size = None
        self.spacing = None
        self.shapes = None
        self.dset_path = os.path.join(self.fpath, 'dataset.tfrecord')
        print(self.dset.keys())

        try:
            self.spacing = self.dset['spacing']
        except:
            pass

        try:
            self.shapes = self.dset['shapes']
        except:
            pass

    def make_train_dataset(self, save_interpolation=None, compression=None,
                           output_path=None):

        if compression:
            options = tf.python_io.TFRecordOptions(
                            tf.python_io.TFRecordCompressionType.GZIP)
        else:
            options = tf.python_io.TFRecordOptions(
                            tf.python_io.TFRecordCompressionType.NONE)
        self.writer = tf.python_io.TFRecordWriter(
                            os.path.join(output_path, 'dataset.tfrecord'),
                            options=options)

        holder = {}
        spacings = {}
        logging.info('Constructing training dataset')
        for img_pair in (self.dset['training']):
            write = True
            holder['idx'] = img_pair['label'].split('./')[-1]
            for img_type, img_path in img_pair.items():
                sitk_img = sitk.ReadImage(
                                os.path.join(self.fpath,
                                             img_path.split('./')[-1]))

                origin_ = sitk_img.GetOrigin()
                orientation_ = sitk_img.GetDirection()
                spacings[img_type] = sitk_img.GetSpacing()
                if img_type != 'label':
                    img = self.resample_image_spacing(
                                    sitk_img, interpolation_='Spline')
                    holder[img_type] = img
                    holder['spacing_orig'] = sitk_img.GetSpacing()
                    holder['shape_orig'] = sitk_img.GetSize()
                elif img_type == 'label':
                    # modalities channel spacing
                    if len(self.spacing) - len(sitk_img.GetSpacing()) != 0:
                        sitk_img.SetSpacing(
                            spacings.pop('image')[:3 - len(self.spacing)])
                    else:
                        sitk_img.SetSpacing(
                            spacings.pop('image'))
                    img = self.resample_image_spacing(
                                sitk_img, interpolation_='nearest')
                    try:
                        holder[img_type] = self.get_multi_class_labels(img)
                    except AssertionError:
                        logging.warning('label conversion failed for {}, \
                                         excluding from training dataset'.format(holder['idx']))
                        write = False
                if save_interpolation:
                    sitk_img_resampled = sitk.GetImageFromArray(img)
                    sitk_img_resampled.SetSpacing(self.spacing)
                    sitk_img_resampled.SetOrigin(origin_)
                    sitk_img_resampled.SetDirection(orientation_)
                    sitk.WriteImage(sitk_img_resampled,
                                    os.path.join(
                                        output_path,
                                        value.split('./')[-1].split('.gz')[0]))
            if write:
                self.write_to_tfrecord(holder)
                logging.info(holder['idx'] + ' processed')
        self.writer.close()

    def scan_dataset_spacing_shapes(self):
        spacings = []
        shapes = []
        for img_pair in (self.dset['training']):
            itkimg = sitk.ReadImage(
                        os.path.join(self.fpath,
                                     img_pair['image'].split('./')[1]))
            spacings.append(itkimg.GetSpacing())
            shapes.append(itkimg.GetSize())
            logging.info(img_pair['image'] + ' scanned')
        self.spacing = np.median(spacings, axis=0)
        self.shapes = np.median(shapes, axis=0)
        # json compatibility
        self.dset['spacing'] = self.spacing.tolist()
        self.dset['shapes'] = self.shapes.tolist()
        logging.info(self.fpath + 'spacing set: {}'.format(self.spacing))
        logging.info(self.fpath + 'shapes set: {}'.format(self.shapes))
        write_json(self.dset, os.path.join(self.fpath, 'dataset.json'))

    def resample_image_spacing(self, itk_image, interpolation_=None):
        itk_image_rank = len(itk_image.GetSpacing())
        if itk_image_rank - len(self.spacing) != 0:
            resampled = resample_to_spacing(
                            itk_image,
                            target_spacing=self.spacing[:itk_image_rank
                                                        - len(self.spacing)],
                            interpolation=interpolation_)
        else:
            resampled = resample_to_spacing(
                            itk_image,
                            target_spacing=self.spacing,
                            interpolation=interpolation_)
        return resampled

    def get_multi_class_labels(self, mask):
        """
        Translates a label map into a set of binary labels.
        :param data: numpy array containing the label map with shape:
                     (n_samples, 1, ...).
        :param n_labels: number of labels.
        :param labels: integer values of the labels.
        :return: binary numpy array of shape: (n_samples, n_labels, ...)
        """
        new_shape = [self.num_classes] + list(mask.shape)
        y = np.zeros(new_shape, np.uint8)
        uniques = np.unique(mask)
        assert (self.num_classes >= len(uniques)), "expected {} number of \
                classes, instead got {} : {}".format(self.num_classes,
                                                     len(np.unique(mask)),
                                                     uniques)
        for label_index in range(self.num_classes):
            y[label_index][mask == label_index] = 1
        return y

    def write_to_tfrecord(self, img_pair):
        try:
            assert(img_pair['image'].dtype == np.float32)

        except AssertionError:
            logging.info(img_pair['idx'] + ': np.float32 expected, got {}, \
            converting to np.float32'.format(img_pair['image'].dtype))
            img = img_pair['image'].astype(np.float32)
            img_pair.pop('image')
            img_pair['image'] = img

        try:
            assert(img_pair['label'].dtype == np.uint8)
        except AssertionError:
            logging.info(img_pair['idx'] + ': np.uint8 expected, got {}, \
            converting to uint8'.format(img_pair['label'].dtype))
            label = img_pair['label'].astype(np.uint8)
            img_pair.pop('label')
            img_pair['label'] = label

        if len(img_pair['image'].shape) == 4:
            try:
                assert(img_pair['image'].shape[1:]
                       == img_pair['label'].shape[1:])
            except AssertionError:
                logging.warning(img_pair['idx'] + ':expected shapes to be \
                                equal, but image was {} and label was {}'.format(
                                    img_pair['image'].shape[1:],
                                    img_pair['label'].shape[1:]))
        else:
            try:
                assert(img_pair['image'].shape == img_pair['label'].shape[1:])
            except AssertionError:
                logging.warning(img_pair['idx'] + ':expected shapes to be \
                                equal, but image was {} and label was {}'.format(
                                    img_pair['image'].shape,
                                    img_pair['label'].shape[1:]))

        raw_image = img_pair['image'].tobytes()
        raw_label = img_pair['label'].tobytes()
        shape_i = np.array(img_pair['image'].shape, np.int32).tobytes()
        shape_l = np.array(img_pair['label'].shape, np.int32).tobytes()

        # 32bit constrained from google protobof
        Kint32Max = 2147483647.0

        if Kint32Max - len(raw_image) - len(raw_label) - len(shape_i) - len(shape_l) < 0:
            logging.warning(
                img_pair['idx'] + ': raw bytecounts > Kint32Max, \
                cropping image and label')

            label = img_pair.pop('label')

            z_min = np.sort(np.unique(np.where(label[1] != 0)[0]))[0]-64
            z_max = np.sort(np.unique(np.where(label[1] != 0)[0]))[-1]+64

            img_pair['label'] = label[:, z_min:z_max]

            img = img_pair.pop('image')

            if len(img.shape) > 3:
                img_pair['image'] = img[:, z_min:z_max]
            else:
                img_pair['image'] = img[z_min:z_max]

            raw_image = img_pair['image'].tobytes()
            raw_label = img_pair['label'].tobytes()

            shape_i = np.array(img_pair['image'].shape, np.int32).tobytes()
            shape_l = np.array(img_pair['label'].shape, np.int32).tobytes()

        shape_i_orig = np.array(img_pair['spacing_orig'], np.int32).tobytes()
        spacing_i_orig = np.array(img_pair['shape_orig'], np.int32).tobytes()

        entry = tf.train.Example(
                features=tf.train.Features(
                    feature={
                        'image': _bytes_feature(raw_image),
                        'shape_i': _bytes_feature(shape_i),
                        'img_dtype': _bytes_feature(
                            tf.compat.as_bytes(str(img_pair['image'].dtype))),
                        'label': _bytes_feature(raw_label),
                        'shape_l': _bytes_feature(shape_l),
                        'label_dtype': _bytes_feature(
                            tf.compat.as_bytes(str(img_pair['label'].dtype))),
                        'idx': _bytes_feature(
                            tf.compat.as_bytes(img_pair['idx'])),
                        'shape_i_orig': _bytes_feature(shape_i_orig),
                        'spacing_i_orig': _bytes_feature(spacing_i_orig)
                    }))
        self.writer.write(entry.SerializeToString())

    def provide_tf_dataset(self, number_epochs, shape=None, batch_size=1,
                           num_parallel_calls=8, mode='train', fraction=0.0,
                           shuffle_size=100, variance=0.1, compression=None,
                           multi_GPU=False):

        self.mode = mode
        self.shape = shape
        self.batch_size = batch_size
        self.fraction = fraction
        self.variance = variance

        if compression:
            compression_type_ = 'GZIP'
        else:
            compression_type_ = ''
        dataset = tf.data.TFRecordDataset(
                    self.dset_path, compression_type=compression_type_)
        dataset = dataset.apply(
                    tf.contrib.data.shuffle_and_repeat(shuffle_size,
                                                       count=number_epochs))
        dataset = dataset.apply(
                    tf.contrib.data.map_and_batch(
                        map_func=self.parse_image,
                        num_parallel_batches=num_parallel_calls,
                        batch_size=batch_size))

        dataset = dataset.prefetch(buffer_size=None)

        if multi_GPU:
            return dataset

        else:
            iterator = dataset.make_one_shot_iterator()
            inputs, outputs = iterator.get_next()
            return inputs, outputs

    def parse_image(self, entry):
        parsed = tf.parse_single_example(entry, features={
                    'image': tf.FixedLenFeature([], tf.string),
                    'shape_i': tf.FixedLenFeature([], tf.string),
                    'img_dtype': tf.FixedLenFeature([], tf.string),
                    'label': tf.FixedLenFeature([], tf.string),
                    'shape_l': tf.FixedLenFeature([], tf.string),
                    'label_dtype': tf.FixedLenFeature([], tf.string),
                    'idx': tf.FixedLenFeature([], tf.string),
                    'shape_i_orig': tf.FixedLenFeature([], tf.string),
                    'spacing_i_orig': tf.FixedLenFeature([], tf.string)
                    })

        # TODO: make dtype flexible
        shape_i = tf.decode_raw(parsed['shape_i'], tf.int32)
        image = tf.decode_raw(parsed['image'], tf.float32)
        image = tf.reshape(image, shape_i)
        image = tf.cond(
                    tf.greater_equal(
                        tf.rank(image), 4),
                    lambda: image,
                    lambda: tf.expand_dims(image, 0))

        shape_l = tf.decode_raw(parsed['shape_l'], tf.int32)
        segmentation = tf.decode_raw(parsed['label'], tf.uint8)
        segmentation = tf.reshape(segmentation, shape_l)

        stacked = tf.concat([image, tf.cast(segmentation, tf.float32)], axis=0)

        paddings = tf.clip_by_value(
                    tf.subtract([self.num_classes]+self.shape, shape_l),
                    0, 100000)
        paddings = tf.ceil(paddings/2)
        paddings_reshaped = tf.cast(tf.stack([paddings, paddings]), tf.int32)
        paddings_reshaped = tf.transpose(paddings_reshaped, [1, 0])
        stacked = tf.cond(
                        tf.reduce_mean(paddings) > 0.0,
                        lambda: tf.pad(stacked, paddings=paddings_reshaped,
                                       mode="reflect"),
                        lambda: stacked)

        cropped_i = stacked[:self.num_modalities, :, :, :]
        cropped_s = stacked[self.num_modalities:, :, :, :]

        cropped_i, cropped_s, rval = tf.py_func(
                                        transforms,
                                        [cropped_i, cropped_s, self.shape, self.fraction, self.spacing, self.variance],
                                        Tout=[tf.float32, tf.float32, tf.float64])
        cropped_s = tf.cast(cropped_s, tf.uint8)

        # static output shape required by tf.dataset.batch() method!
        cropped_i.set_shape([self.num_modalities] + self.shape)
        cropped_s.set_shape([self.num_classes] + self.shape)
        print("input shape" + str(cropped_i.shape))
        print("segmentation shape" + str(cropped_s.shape))

        objects = tf.where(cropped_s[1:] > 0)
        objects_in_crop = tf.reduce_mean(objects, axis=0)
        background = tf.where(cropped_s[0] > 0)
        ratio = tf.shape(objects)[0] / tf.shape(background)[0]
        objects_in_crop = tf.cond(
                            tf.shape(objects)[0] > 0,
                            lambda: objects_in_crop,
                            lambda: tf.zeros_like(objects_in_crop))
        tf.Print([], [ratio], message="ratio of fore to background in patch: ")

        if self.mode == 'train':
            return({'img': cropped_i, 'object_in_crop': objects_in_crop},
                   cropped_s)
        elif self.mode == 'dataloader_test':
            tf.Print(['img shape'], [tf.shape(stacked[:self.num_modalities])])
            return({'img': cropped_i, 'rval': rval},
                   cropped_s)

if __name__ == '__main__':

    parser = argparse.ArgumentParser(description='generate dataset for'
                                     'MSD_challenge')
    parser.add_argument('-i', '--input', help='input (root) path',
                        required=True)
    parser.add_argument('-o', '--output',
                        help='path to store generated TFRecord and json',
                        required=True)
    parser.add_argument('-s', '--save_interpolation',
                        help='save interpolated images',
                        required=False,
                        default=None)
    parser.add_argument('-c', '--compression',
                        help='compress generated TFRecord',
                        required=False,
                        default=None)

    args = vars(parser.parse_args())

    fpath = args['input']
    output_path = args['output']
    compression = args['compression']
    save_images = args['save_interpolation']

    start_logger(os.path.join(fpath + 'dataset_generation'))
    logger = logging.getLogger(__name__)
    dset = TFRdataset(fpath)
    if dset.spacing is None or dset.shapes is None:
        dset.scan_dataset_spacing_shapes()
    dset.make_train_dataset(output_path=output_path,
                            compression=compression,
                            save_interpolation=save_images)

ext: py

reason: import numpy