full.py

#!/usr/bin/env python

import socket
import struct
import time
import threading
import os
import sys
import math
import numpy as np
import cv2
from robots import UR5
from cameras import RealSense
import pybullet as p
import utils_3d as utils
from skimage import measure
from scipy import optimize
import matplotlib.pyplot as plt
import utils_3d


class RealUR5Env(object):

    def __init__(self, args):
        self.is_sim = False
        self.task = args.task
        self.last_reset_iter = 0
        # Defines where robot end effector can move to in world coordinates
        self._workspace_bounds=np.array([[ 0.35,  0.85], # 3x2 rows: x,y,z cols: min,max
                                         [-0.30,  0.05],
                                         [-0.4,  -0.1]])

        # Defines where robot can see in world coordinates (3D bounds for TSDF fusion)
        self._view_bounds = np.array([[ 0.3,   0.9], # 3x2 rows: x,y,z cols: min,max
                                      [-0.35,  0.1],
                                      [-0.4,  -0.1]])

        self._home_j_config = np.asarray([0.0, -90.0, -90.0, -90.0, 90.0, 0.0]) * np.pi / 180.0

        if args.task =='grasp':
            self.robot_ip ='xxx'
            tool_offset = [0.0,0.0,0.174,0.0,0.0,0.0]
            cali_file_name = '_g.txt'
            self._bin_cam = RealSense(tcp_ip='127.0.0.1',tcp_port=50010,im_h=720,im_w=1280,max_depth=3.0)
            self._heightmap_pixel_size = 0.00125 # in meters
            self._block_offset_from_tool = np.asarray([0.0, -0.015/np.sqrt(2.), 0.015/np.sqrt(2.)]) # from tip to checkerboard center
            #self._block_offset_from_tool = np.asarray([0.0, -0.018, 0.0]) # from tip to checkerboard center
            self._tool_orientation = np.asarray([0.006,-2.899,1.216])
            #self._tool_orientation = np.asarray([0.0,-3.14,0.00])
        elif args.task == 'suction':
            self.robot_ip ='xxx'
            tool_offset = [0e-3, 8e-3, 236e-3, 0e-3, 0e-3, 0e-3]
            cali_file_name = '_s.txt'
            self._bin_cam = RealSense(tcp_ip='127.0.0.1',tcp_port=50010,im_h=720,im_w=1280,max_depth=3.0)
            #self._heightmap_pixel_size = 0.005 # in meters
            self._heightmap_pixel_size = 0.00125 # in meters

            # Workspace for calibration
            self._workspace_bounds = np.array([[0.4, 0.8], [-0.37, 0.25], [-0.45, -0.3]])
            self._view_bounds = np.array([[0.4, 0.8], [-0.43, 0.27], [-0.5, -0.3]])

            self._block_offset_from_tool = np.asarray([0.03, 0.0, 0.11]) # from tip to checkerboard center
            self._tool_orientation = np.asarray([2.222,-2.2213,0.01])
            #self._tool_orientation = np.asarray([1.2,1.2,1.2])

        self.computer_ip = '128.30.31.8'
        self.robot = UR5(tcp_ip=self.robot_ip,tcp_port=30002,rtc_port=30003,computer_ip=self.computer_ip,
                         j_acc=1.4,j_vel=1.1, # safer params: j_acc=1.4,j_vel=1.1,
                         #j_acc=1.1,j_vel=2.0, # safer params: j_acc=1.4,j_vel=1.1,
                         tool_offset=tool_offset,
                         home_j_config=self._home_j_config,
                         ar_tracker=None,rtde_config_path='./record_configuration.xml')

        try:
            print('./calibration/camera_pose'+cali_file_name)
            self._bin_cam_pose = np.loadtxt('./calibration/camera_pose'+cali_file_name)
            self._bin_cam_depth_scale = np.loadtxt('./calibration/camera_depth_scale'+cali_file_name)
        except:
            self.robot.homej(blocking=True)
            print('Calibration files not found! Running calibration')
            input("Press Enter to continue ...")
            self.calibrate_bin_cam(cali_file_name)



        # Define Denavit-Hartenberg parameters for UR5
        self._ur5_kinematics_d = np.array([0.1625, 0.     , 0.      , 0.1333, 0.0997, 0.0996])
        self._ur5_kinematics_a = np.array([0.      ,-0.42500,-0.39225, 0.     , 0.     , 0.    ])

        # Move robot to home configuration
        self.open_gripper(blocking=True)
        self.robot.homej(blocking=True)
        self.initial_gripper_pose = self.robot.get_tool_pose()[0:3,0:4]


        # Start thread to perform robot actions in parallel to main thread
        # self.robot_do_grasp,self.robot_is_grasping = False,False
        # self.robot_grasp_params = [None,None] # grasping primitive parameters
        # self.robot_grasp_success,self.robot_grasped_object_shape_id,self.robot_grasp_object_offset = None,-1,np.array([0,0,0])
        # self.robot_do_toss,self.robot_is_tossing = False,False
        # self.robot_toss_params = [None,None] # tossing primitive parameters
        # action_thread = threading.Thread(target=self.robot_do_action)
        # action_thread.daemon = True
        # action_thread.start()




    # Callback for doing robot actions in parallel with main thread
    # def robot_do_action(self):
    #     # while True:
    #     #     if not self._ar_tracker.marker_detected:
    #     #         if self.robot_do_grasp:
    #     #             self.robot_is_grasping = True
    #     #             grasp_position = self.robot_grasp_params[0]
    #     #             grasp_angle = self.robot_grasp_params[1]
    #     #             self.robot_grasp_success = self.robot.grasping_primitive(grasp_position,grasp_angle)
    #     #             self.robot_do_grasp = False
    #     #             self.robot_is_grasping = False
    #     #     time.sleep(0.01)
    #     time.sleep(0.01)

    def init_bounds(self):
        color_image, depth_image, cam_pose = self.get_gripper_camera()
        z_near = 0.2
        z_far = 3.5
        depth_image[depth_image<z_near] = 0
        depth_image[depth_image>z_far] = 0

        camera_points,color_points = utils.get_pointcloud(color_image,depth_image,self.get_camera_intrisic())
        xyz_points = utils.transform_pointcloud(camera_points,cam_pose)
        # denoise xyz_points
        bnd = utils.denoise_pointcloud(xyz_points,3)
        view_bounds = bnd.copy()
        view_bounds[:,0] = view_bounds[:,0]-0.05
        view_bounds[:,1] = view_bounds[:,1]+0.05


        gripper_tranformation = self.robot.get_tool_pose()

        workspace_bounds = view_bounds.copy()
        workspace_bounds[:,0] = np.min(np.vstack((workspace_bounds[:,0],gripper_tranformation[0:3,3])), axis= 0)
        workspace_bounds[:,1] = np.max(np.vstack((workspace_bounds[:,1],gripper_tranformation[0:3,3])), axis= 0)
        return view_bounds,workspace_bounds

    def get_view_bounds(self):
        return self._view_bounds

    def get_workspace_bounds(self):
        return  self._workspace_bounds

    def robot_go_home(self,blocking=True):
        self.robot.homej(blocking)

    def get_bin_camera_data(self):
        color_im = self._bin_cam.color_im.copy()
        depth_im = self._bin_cam.depth_im.copy()*self._bin_cam_depth_scale
        return color_im,depth_im

    def get_bin_heightmap(self,color_image,depth_image):
        camera_points,color_points = utils.get_pointcloud(color_image,depth_image,self._bin_cam.color_intr)
        camera_points = utils.transform_pointcloud(camera_points,self._bin_cam_pose)
        # utils.write_pointcloud('test.ply',camera_points,color_points)
        color_heightmap,depth_heightmap = utils.get_heightmap(camera_points,color_points,self._view_bounds,self._heightmap_pixel_size,zero_level=self._view_bounds[2,0])
        return color_heightmap,depth_heightmap

    def get_valid_pixels_heightmap(self,depth_heightmap):

        heightmap_width = depth_heightmap.shape[1]
        heightmap_height = depth_heightmap.shape[0]

        # Get 3D locations of each heightmap pixel in world coordinates
        heightmap_pixel_x,heightmap_pixel_y = np.meshgrid(np.linspace(0,heightmap_width-1,heightmap_width),
                                                          np.linspace(0,heightmap_height-1,heightmap_height))
        heightmap_points = np.array([heightmap_pixel_x*self._heightmap_pixel_size+self._view_bounds[0,0],
                                     heightmap_pixel_y*self._heightmap_pixel_size+self._view_bounds[1,0],
                                     depth_heightmap+self._view_bounds[2,0]]).transpose(1,2,0)
        within_workspace_bounds = np.logical_and(heightmap_points[:,:,0] >= self._workspace_bounds[0,0],
                                  np.logical_and(heightmap_points[:,:,0] <= self._workspace_bounds[0,1],
                                  np.logical_and(heightmap_points[:,:,1] >= self._workspace_bounds[1,0],
                                  np.logical_and(heightmap_points[:,:,1] <= self._workspace_bounds[1,1],
                                  np.logical_and(heightmap_points[:,:,2] >= self._workspace_bounds[2,0],
                                                 heightmap_points[:,:,2] <= self._workspace_bounds[2,1])))))
        return within_workspace_bounds

    def get_grasp_position(self,grasp_pixel,depth_heightmap):
        valid_pixels = np.array(np.where(depth_heightmap > 0)).T # y,x
        dist_to_valid = np.sqrt(np.sum((valid_pixels-grasp_pixel)**2,axis=1))
        closest_valid_pixel = grasp_pixel
        if np.min(dist_to_valid) < 2: # get nearest non-zero pixel less than 2 pixels away
            closest_valid_pixel = valid_pixels[np.argmin(dist_to_valid),:]
        grasp_position = np.array([closest_valid_pixel[1]*self._heightmap_pixel_size+self._view_bounds[0,0],
                                   closest_valid_pixel[0]*self._heightmap_pixel_size+self._view_bounds[1,0],
                                   depth_heightmap[closest_valid_pixel[0],closest_valid_pixel[1]]+self._view_bounds[2,0]])

        grasp_position = np.clip(grasp_position,self._workspace_bounds[:,0],self._workspace_bounds[:,1]) # clamp grasp position w.r.t. workspace bounds
        return grasp_position,closest_valid_pixel

    # Grasp for simulation
    def grasp(self, grasp_pixel, grasp_angle, depth_heightmap):
        grasp_position,closest_valid_pixel = self.get_grasp_position(grasp_pixel,depth_heightmap)
        grasp_position[2] -= 0.03 # target 3cm below observed surface
        print("Grasp position: {}".format(grasp_position))
        robot_grasp_success = self.robot.grasping_primitive(grasp_position,grasp_angle)
        return robot_grasp_success

    # Grasp for real robot
    def grasp_real(self, grasp_position, grasp_angle):
        grasp_position[2] -= 0.04 # target 3cm below observed surface

        bottom = -0.36
        if grasp_position[2] < bottom:
            grasp_position[2] = bottom
        print("Grasp position: {}".format(grasp_position))
        robot_grasp_success = self.robot.grasping_primitive(grasp_position, grasp_angle)
        return robot_grasp_success

    # Suction for simulation
    def suction(self, grasp_pixel, grasp_angle, depth_heightmap):
        grasp_position,closest_valid_pixel = self.get_grasp_position(grasp_pixel,depth_heightmap)
        grasp_position[2] += 0.015 # target 1.5cm above observed surface
        print("Grasp position: {}".format(grasp_position))
        robot_suction_success = self.robot.suction_primitive(grasp_position)
        return robot_suction_success

    # Suction for real robot
    def suction_real(self, grasp_position):
        if grasp_position[2] < self._workspace_bounds[2, 0]:
            grasp_position[2] = self._workspace_bounds[2, 0]
        print("Grasp position: {}".format(grasp_position))
        robot_suction_success = self.robot.suction_primitive(grasp_position)
        return robot_suction_success

    def move2goal(self,relativeRT,max_pixel,render_depth,cam_pose_vir,camera_intrin_vir,step_size):
        curr_transformation = self.robot.get_tool_pose()
        relativeR = np.zeros((3,4))
        relativeR[0:3,0:3] = relativeRT[0:3,0:3]
        move_rot = utils_3d.tranformRT(relativeR,curr_transformation)

        target_3d_point = utils_3d.get_3d_point(max_pixel.reshape(2,1),render_depth,cam_pose_vir,camera_intrin_vir)
        workspace_bounds_tool = self.get_workspace_bounds()


        target_3d_point = np.clip(target_3d_point,workspace_bounds_tool[:,0],workspace_bounds_tool[:,1])
        # tool_offest = np.array(self.tool_tip_to_ee_joint)
        # tool_offest[2] = tool_offest[2] - 0.035 #scale = 0.125
        target_3d_tool_pos = target_3d_point.copy() #-np.dot(move_rot[0:3,0:3],tool_offest.reshape(3,1)).reshape(1,3)


        move_vec = target_3d_tool_pos - move_rot[0:3,3].T
        distance_to_goal = np.linalg.norm(move_vec)
        move_vec_step = move_vec *(min(distance_to_goal,step_size)/distance_to_goal)

        mov_loc = move_rot[0:3,3] + move_vec_step.reshape(3)
        arrive_goal = int(distance_to_goal<step_size)


        if not arrive_goal:
            offset = -1*self.initial_gripper_pose[:,2]*0.05
            target_tool_t = np.array(mov_loc).flatten()+ offset
            target_tool_rot = utils.angle2urx(utils.rotm2angle(move_rot[0:3,0:3]))
            succ = self.robot.movej(use_pos=True, params=[target_tool_t[0],target_tool_t[1],target_tool_t[2],
                                                        target_tool_rot[0],target_tool_rot[1],target_tool_rot[2]], blocking=True)

        target_tool_t = np.array(mov_loc).flatten()
        target_tool_rot = utils.angle2urx(utils.rotm2angle(move_rot[0:3,0:3]))
        succ = self.robot.movej(use_pos=True, params=[target_tool_t[0],target_tool_t[1],target_tool_t[2],
                                                        target_tool_rot[0],target_tool_rot[1],target_tool_rot[2]], blocking=True)
        if succ == 0:
            self.robot.homej(True)


        print("target_3d_point:{}".format(target_3d_point))
        print(self.get_current_tool_tip_location()[:,3])
        # print(target_3d_tool_pos)
        # input()


        return succ,curr_transformation,arrive_goal

    def get_current_tool_tip_location(self):
        curr_transformation = self.robot.get_tool_pose()
        return curr_transformation[0:3,0:4]

    def open_gripper(self,blocking=False):
        if self.task=='grasp':
            self.robot.open_gripper(blocking)
        elif self.task=='suction':
            self.robot.off_suction(blocking)

    def close_gripper(self,blocking=False):
        if self.task=='grasp':
            self.robot.close_gripper(blocking)
        elif self.task=='suction':
            self.robot.on_suction(blocking)

    def execute_place(self,place_pos=None,place_angle=-90.,):
        if place_pos is None:
            if self.task=='grasp':
                place_pos = np.array([0.55, 0.6, 0.0])
            elif self.task=='suction':
                place_pos = np.array([0.56,0.54,-0.2])

        self.robot.homej(blocking=True)
        # If we want to place the objects using gripper's initial pose, uncomment the following lime:
        # target_tool_rot = utils.angle2urx(utils.rotm2angle(self.initial_gripper_pose[0:3,0:3]))
        
        home_rot = np.asarray([np.cos(7*np.pi/4),np.sin(7*np.pi/4),0.0])*np.pi
        home_rotm = utils.angle2rotm(utils.urx2angle(home_rot))
        targ_rotm = np.dot(utils.angle2rotm([place_angle*np.pi/180.,0,0,1.]),home_rotm)
        target_tool_rot = utils.angle2urx(utils.rotm2angle(targ_rotm))
        succ = self.robot.movej(use_pos=True,
                                params=[place_pos[0],place_pos[1],place_pos[2],
                                        target_tool_rot[0],target_tool_rot[1],target_tool_rot[2]],
                                blocking=True)


        #succ = self.robot.movej(use_pos=True,params=[place_pos_b[0],place_pos_b[1],place_pos_b[2],target_tool_rot[0],target_tool_rot[1],target_tool_rot[2]],blocking=True)
        self.open_gripper(blocking=True)

        #succ = self.robot.movej(use_pos=True,params=[place_pos_a[0],place_pos_a[1],place_pos_a[2],target_tool_rot[0],target_tool_rot[1],target_tool_rot[2]],blocking=True)

    def reset_objects(self, curr_iter, auto=True):
        if auto:
            self.robot.reset_primitive()
        else:  # manually reset the objects
            print('Not enough stuff in view! Resetting objects!! Press Enter When Done')
            self.robot.homej(blocking=True)
            input()
        self.last_reset_iter = curr_iter

    def calibrate_bin_cam(self,cali_file_name):

        # Set limits on y-axis
        self._workspace_bounds[1,1] = min(self._workspace_bounds[1,1],0.3)

        # Move robot to home configuration
        self.robot.set_speed(j_acc=1.4,j_vel=1.1)
        self.robot.close_gripper(blocking=True)
        self.robot.homej(blocking=True)

        # self.robot.movej(use_pos=False,params=np.asarray([0.0., -90., -90., -90., 45, 90.])*np.pi/180.,blocking=True)

        # Constants
        calib_grid_step = 0.05
        tool_orientation = self._tool_orientation
        # block_offset_from_tool = np.asarray([0.,(0.044/2)/np.sqrt(2)-(0.022/2)/np.sqrt(2),(0.045/2)/np.sqrt(2)+(0.022/2)/np.sqrt(2)]) # from tip to block center


        # Construct 3D calibration grid across workspace
        gridspace_x = np.linspace(self._workspace_bounds[0,0]+0.1,self._workspace_bounds[0,1]-0.1,1+(self._workspace_bounds[0,1]-self._workspace_bounds[0,0]-0.2)/calib_grid_step)
        gridspace_y = np.linspace(self._workspace_bounds[1,0]+0.1,self._workspace_bounds[1,1]-0.1,1+(self._workspace_bounds[1,1]-self._workspace_bounds[1,0]-0.2)/calib_grid_step)
        calib_grid_x,calib_grid_y,calib_grid_z = np.meshgrid(gridspace_x,gridspace_y,self._workspace_bounds[2,0]+0.1)
        num_calib_grid_pts = calib_grid_x.shape[0]*calib_grid_x.shape[1]*calib_grid_x.shape[2]
        calib_grid_x.shape = (num_calib_grid_pts,1)
        calib_grid_y.shape = (num_calib_grid_pts,1)
        calib_grid_z.shape = (num_calib_grid_pts,1)
        calib_grid_pts = np.concatenate((calib_grid_x,calib_grid_y,calib_grid_z), axis=1)

        # Move robot to each calibration point in workspace
        global measured_pts, observed_pts, observed_pix, world2camera
        measured_pts = []
        observed_pts = []
        observed_pix = []
        print('Collecting data ...')
        for calib_pt_idx in range(num_calib_grid_pts):
            tool_position = calib_grid_pts[calib_pt_idx,:]
            print(tool_position, tool_orientation)

            self.robot.movej(use_pos=True,params=[tool_position[0],tool_position[1],tool_position[2],tool_orientation[0],tool_orientation[1],tool_orientation[2]],blocking=True)
            time.sleep(1.0)

            chckr_found = False
            while not chckr_found:

                # Find pixel location of red block
                color_im,depth_im = self._bin_cam.color_im,self._bin_cam.depth_im
                # plt.imshow(color_im)
                # plt.show()

                chckr_size = (3,3)
                refine_criteria = (cv2.TERM_CRITERIA_EPS+cv2.TERM_CRITERIA_MAX_ITER,30,0.001)
                bgr_im = cv2.cvtColor(color_im,cv2.COLOR_RGB2BGR)
                gray_im = cv2.cvtColor(bgr_im,cv2.COLOR_RGB2GRAY)
                chckr_found, crnrs = cv2.findChessboardCorners(gray_im,chckr_size,None,cv2.CALIB_CB_ADAPTIVE_THRESH)
                if chckr_found:
                    crnrs_refined = cv2.cornerSubPix(gray_im,crnrs,(3,3),(-1,-1),refine_criteria)
                    block_pix = crnrs_refined[4,0,:]
                else:
                    time.sleep(0.01)
                    continue

                # conf_map = color_im[:,:,0]/255.*(1-color_im[:,:,1]/255.)*(1-color_im[:,:,2]/255.)
                # conf_map = cv2.GaussianBlur(conf_map,(101,101),0)
                # block_pix = np.round(np.unravel_index(np.argmax(conf_map),conf_map.shape)).astype(int) # y,x
                # block_pix = np.flip(block_pix,axis=0) # save in fromat: x,y

            # Get observed checkerboard center 3D point in camera space
            block_z = depth_im[int(np.round(block_pix[1])),int(np.round(block_pix[0]))]
            block_x = np.multiply(block_pix[1]-self._bin_cam.color_intr[0,2],block_z/self._bin_cam.color_intr[0,0])
            block_y = np.multiply(block_pix[0]-self._bin_cam.color_intr[1,2],block_z/self._bin_cam.color_intr[1,1])
            print("Found {}".format([block_x,block_y,block_z]))
            if block_z == 0:
                print("Zero Z, potential error!")
                continue

            # Save calibration point and observed checkerboard center
            observed_pts.append([block_x,block_y,block_z])
            tool_position += self._block_offset_from_tool
            measured_pts.append(tool_position)
            observed_pix.append(block_pix)

            # Draw and display the corners
            vis_im = cv2.circle(color_im,(block_pix[0],block_pix[1]),7,(0,255,0),2)
            # cv2.imwrite('%06d.png' % len(measured_pts), vis)
            cv2.imshow('Calibration',cv2.cvtColor(vis_im,cv2.COLOR_RGB2BGR))
            cv2.waitKey(10)

        # Move robot back to home pose
        self.robot.homej(blocking=True)

        measured_pts = np.asarray(measured_pts)
        observed_pts = np.asarray(observed_pts)
        observed_pix = np.asarray(observed_pix)
        world2camera = np.eye(4)

        # Estimate rigid transform with SVD (from Nghia Ho)
        def get_rigid_transform(A, B):
            assert len(A) == len(B)
            N = A.shape[0]; # Total points
            centroid_A = np.mean(A, axis=0)
            centroid_B = np.mean(B, axis=0)
            AA = A - np.tile(centroid_A, (N, 1)) # Centre the points
            BB = B - np.tile(centroid_B, (N, 1))
            H = np.dot(np.transpose(AA), BB) # Dot is matrix multiplication for array
            U, S, Vt = np.linalg.svd(H)
            R = np.dot(Vt.T, U.T)
            if np.linalg.det(R) < 0: # Special reflection case
               Vt[2,:] *= -1
               R = np.dot(Vt.T, U.T)
            t = np.dot(-R, centroid_A.T) + centroid_B.T
            return R, t

        def get_rigid_transform_error(z_scale):
            global measured_pts, observed_pts, observed_pix, world2camera

            # Apply z offset and compute new observed points using camera intrinsics
            print(observed_pts, z_scale)
            observed_z = observed_pts[:,2:] * z_scale
            observed_x = np.multiply(observed_pix[:,[0]]-self._bin_cam.color_intr[0,2],observed_z/self._bin_cam.color_intr[0,0])
            observed_y = np.multiply(observed_pix[:,[1]]-self._bin_cam.color_intr[1,2],observed_z/self._bin_cam.color_intr[1,1])
            new_observed_pts = np.concatenate((observed_x, observed_y, observed_z), axis=1)

            # Estimate rigid transform between measured points and new observed points
            R, t = get_rigid_transform(np.asarray(measured_pts), np.asarray(new_observed_pts))
            t.shape = (3,1)
            world2camera = np.concatenate((np.concatenate((R, t), axis=1),np.array([[0, 0, 0, 1]])), axis=0)

            # Compute rigid transform error
            registered_pts = np.dot(R,np.transpose(measured_pts)) + np.tile(t,(1,measured_pts.shape[0]))
            error = np.transpose(registered_pts) - new_observed_pts
            error = np.sum(np.multiply(error,error))
            rmse = np.sqrt(error/measured_pts.shape[0]);
            return rmse

        # Optimize z scale w.r.t. rigid transform error
        print('Calibrating ...')
        z_scale_init = 1
        optim_result = optimize.minimize(get_rigid_transform_error, np.asarray(z_scale_init), method='Nelder-Mead')
        camera_depth_offset = optim_result.x

        # Save camera optimized offset and camera pose
        print('Saving calibration files ...')
        if not os.path.exists('./calibration'):
            os.makedirs('./calibration')
        np.savetxt('./calibration/camera_depth_scale'+cali_file_name, camera_depth_offset, delimiter=' ')
        get_rigid_transform_error(camera_depth_offset)
        camera_pose = np.linalg.inv(world2camera)
        np.savetxt('./calibration/camera_pose'+cali_file_name, camera_pose, delimiter=' ')
        print('Done. Please restart main script.' + './calibration/camera_pose'+cali_file_name)

    def touch(self):

        self.robot.set_speed(j_acc=1.4,j_vel=1.1)
        self.robot.homej(blocking=True)

        tool_orientation = np.asarray([np.cos(7*np.pi/4),np.sin(7*np.pi/4),0.0])*np.pi

        # Callback function for clicking on OpenCV window
        global click_point_pix
        click_point_pix = ()
        color_im,depth_im = self._bin_cam.color_im,self._bin_cam.depth_im
        def mouseclick_callback(event, x, y, flags, param):
            if event == cv2.EVENT_LBUTTONDOWN:
                global click_point_pix
                click_point_pix = (x,y)

                # Get click point in camera coordinates
                click_z = depth_im[y,x]*self._bin_cam_depth_scale
                click_x = (x-self._bin_cam.color_intr[0,2])*click_z/self._bin_cam.color_intr[0,0]
                click_y = (y-self._bin_cam.color_intr[1,2])*click_z/self._bin_cam.color_intr[1,1]
                if click_z == 0:
                    return
                click_point = np.asarray([click_x,click_y,click_z])
                click_point = np.append(click_point,1.0).reshape(4,1)

                # Convert camera coordinates to robot coordinates
                target_position = np.dot(self._bin_cam_pose,click_point)
                target_position = target_position[0:3,0]
                print(target_position)

                # Move robot to target position
                final_pose = np.asarray([target_position[0],target_position[1],target_position[2],tool_orientation[0],tool_orientation[1],tool_orientation[2]])
                above_pose = final_pose.copy()
                above_pose[2] += 0.1
                self.robot.movej(use_pos=True,params=above_pose,blocking=True)
                self.robot.movej(use_pos=True,params=final_pose,blocking=True)
                time.sleep(3.0)

                # Move robot back to home position
                self.robot.movej(use_pos=True,params=above_pose,blocking=True)
                self.robot.homej(blocking=True)

        # Show color and depth frames
        cv2.namedWindow('color')
        cv2.setMouseCallback('color', mouseclick_callback)

        while True:
            color_im,depth_im = self._bin_cam.color_im,self._bin_cam.depth_im
            bgr_data = cv2.cvtColor(color_im, cv2.COLOR_RGB2BGR)
            if len(click_point_pix) != 0:
                bgr_data = cv2.circle(bgr_data, click_point_pix, 7, (0,0,255), 2)
            cv2.imshow('color', bgr_data)

            if cv2.waitKey(1) == ord('c'):
                break