def test_combined():

    a = 0.15
    v = 0.08
    urc.movel_wait(pose0, a=a, v=v)
    rx_move(1200)
    time.sleep(2)

    gs = GelSight(
        IP=IP,
        corners=corners,
        tracking_setting=tracking_setting,
        output_sz=(400, 300),
        id="right",
    )
    gs.start()
    c = input()

    rx_move(760)
    grc.follow_gripper_pos = 1
    time.sleep(0.5)

    depth_queue = []

    cnt = 0
    dt = 0.05
    pos_x = 0.5

    tm_key = time.time()
    logger = Logger()
    noise_acc = 0.
    flag_record = False
    tm = 0
    start_tm = time.time()

    vel = [0.00, 0.008, 0, 0, 0, 0]

    while True:
        img = gs.stream.image

        # get pose image
        pose_img = gs.pc.pose_img
        # pose_img = gs.pc.frame_large
        if pose_img is None:
            continue

        # depth_current = gs.pc.depth.max()
        # depth_queue.append(depth_current)
        #
        # if len(depth_queue) > 2:
        #     depth_queue = depth_queue[1:]
        #
        # if depth_current == np.max(depth_queue):
        pose = gs.pc.pose
        cv2.imshow("pose", pose_img)

        if gs.pc.inContact:

            # if cnt % 4 < 2:
            #     # grc.follow_gripper_pos = 1
            #     rx_move(810)
            # else:
            a = 0.02
            v = 0.02

            fixpoint_x = pose0[0]
            fixpoint_y = pose0[1] - 0.133
            pixel_size = 0.2e-3
            ur_pose = urc.getl_rt()
            ur_xy = np.array(ur_pose[:2])

            x = 0.1 - pose[0] - 0.5 * (1 - 2 * pose[1]) * np.tan(pose[2])
            alpha = np.arctan(ur_xy[0] - fixpoint_x) / (
                ur_xy[1] - fixpoint_y) * np.cos(np.pi * ang / 180)

            print("x: ", x, "; input: ", x * pixel_size)

            # K = np.array([6528.5, 0.79235, 2.18017]) #10 degrees
            # K = np.array([7012, 8.865, 6.435]) #30 degrees
            # K = np.array([1383, 3.682, 3.417])
            K = np.array([862689, 42.704, 37.518])

            state = np.array([[x * pixel_size], [pose[2]], [alpha]])
            phi = -K.dot(state)

            # noise = random.random() * 0.07 - 0.02
            # a = 0.8
            # noise_acc = a * noise_acc + (1 - a) * noise
            # phi += noise_acc

            target_ur_dir = phi + alpha
            limit_phi = np.pi / 3
            target_ur_dir = max(-limit_phi, min(target_ur_dir, limit_phi))
            if abs(target_ur_dir) == limit_phi:
                print("reached phi limit")
            v_norm = 0.02
            vel = np.array([
                v_norm * sin(target_ur_dir) * cos(np.pi * ang / 180),
                v_norm * cos(target_ur_dir),
                v_norm * sin(target_ur_dir) * sin(np.pi * -ang / 180), 0, 0, 0
            ])

            # if x < -0.2:
            #     print("regrasp")
            #     rx_regrasp()

            if ur_pose[0] < -0.7 - .1693:
                vel[0] = max(vel[0], 0.)
                print("reached x limit")
            if ur_pose[0] > -0.4 - .1693:
                vel[0] = min(vel[0], 0.)
                print("reached x limit")
            if ur_pose[2] < .15:
                vel[2] = 0.
                print("reached z limit")
            if ur_pose[1] > .34:
                print("end of workspace")
                print("log saved: ", logger.save_logs())
                gs.pc.inContact = False
                vel[0] = min(vel[0], 0.)
                vel[1] = 0.

            # print("sliding vel ", vel[0], "posx ", pos_x)

            vel = np.array(vel)
            urc.speedl(vel, a=a, t=dt * 2)

            time.sleep(dt)

        else:
            print("no pose estimate")
            print("log saved: ", logger.save_logs())
            break

        # # get tracking image
        # tracking_img = gs.tc.tracking_img
        # if tracking_img is None:
        #     continue

        # slip_index_realtime = gs.tc.slip_index_realtime
        # print("slip_index_realtime", slip_index_realtime)

        # cv2.imshow("marker", tracking_img[:, ::-1])
        # cv2.imshow("diff", gs.tc.diff_raw[:, ::-1] / 255)

        # if urc.getl_rt()[0] < -.45:
        #     break

        # cnt += 1

        c = cv2.waitKey(1) & 0xFF
        if c == ord("q"):
            break

        ##################################################################
        # Record data
        # 'gelsight_url'  : self.gelsight_url,
        # 'fabric_pose'   : self.fabric_pose,
        # 'ur_velocity'   : self.ur_velocity,
        # 'ur_pose'       : self.ur_pose,
        # 'slip_index'    : self.slip_index,
        # 'x'             : self.x,
        # 'y'             : self.x,
        # 'theta'         : self.theta,
        # 'phi'           : self.phi,
        # 'dt'            : self.dt

        if gs.pc.inContact:
            # print("LOGGING")
            # logger.gelsight = gs.pc.diff
            # logger.cable_pose = pose
            logger.ur_velocity = vel
            logger.ur_pose = urc.getl_rt()

            v = np.array([logger.ur_velocity[0], logger.ur_velocity[1]])
            alpha = asin(v[1] / np.sum(v**2)**0.5)

            logger.x = pose[0]
            logger.y = pose[1]
            logger.theta = pose[2]
            # logger.phi = alpha - logger.theta

            logger.dt = time.time() - tm
            tm = time.time()

            logger.add()
Esempio n. 2
0
def test_combined():

    a = 0.15
    v = 0.08
    urc.movel_wait(pose0, a=a, v=v)
    rx_move(1200)
    time.sleep(2)

    gs = GelSight(
        IP=IP,
        corners=corners,
        tracking_setting=tracking_setting,
        output_sz=(400, 300),
        id="right",
    )
    gs.start()
    c = input()

    rx_move(800)
    grc.follow_gripper_pos = 1
    time.sleep(0.5)

    depth_queue = []

    cnt = 0
    dt = 0.05
    pos_x = 0.5

    tm_key = time.time()
    logger = Logger()
    noise_acc = 0.
    flag_record = False
    tm = 0
    start_tm = time.time()

    vel = [0.00, 0.008, 0, 0, 0, 0]

    while True:
        img = gs.stream.image

        # get pose image
        pose_img = gs.pc.pose_img
        # pose_img = gs.pc.frame_large
        if pose_img is None:
            continue

        # depth_current = gs.pc.depth.max()
        # depth_queue.append(depth_current)
        #
        # if len(depth_queue) > 2:
        #     depth_queue = depth_queue[1:]
        #
        # if depth_current == np.max(depth_queue):
        pose = gs.pc.pose
        cv2.imshow("pose", pose_img)

        if gs.pc.inContact:

            # if cnt % 4 < 2:
            #     # grc.follow_gripper_pos = 1
            #     rx_move(810)
            # else:
            a = 0.02
            v = 0.02
            kp = .03
            # kp_rot = .2

            # pos_x = (2*pose[0] + (1 - pose[1])*np.tan(pose[2]))/2
            pos_x = (pose[0] + (1 - pose[1]) * np.tan(pose[2]))
            # pos_x = pose[0]
            # e = (pos_x-0.5)*kp

            # vel = [0, (pos_x-0.3)*kp, -0.008, 0, 0, 0]
            # vel = [0, (pos_x-0.6)*kp, -0.008, kp_rot*gs.pc.pose[2], 0, 0]
            # vel = [0, e*np.cos(th) - dy, -e*np.sin(th) - dz, kp_rot*gs.pc.pose[2], 0, 0]

            noise = random.random() * 0.03 - 0.015
            a = 0.8
            noise_acc = a * noise_acc + (1 - a) * noise
            print(pos_x)

            vel = [(pos_x - 0.10) * kp + noise_acc, 0.008,
                   -(pos_x - 0.10) * kp * .2, 0, 0, 0]
            vel = np.array(vel)

            # grc.follow_gripper_pos = .885
            # grc.follow_gripper_pos = .88
            # rx_move(830)
            # urc.speedl([(pose[0]-0.2)*kp, 0.008, 0, 0, 0, 0], a=a, t=dt*2)

            ur_pose = urc.getl_rt()
            if ur_pose[0] < -0.7:
                vel[0] = max(vel[0], 0.)
            if ur_pose[0] > -0.3:
                vel[0] = min(vel[0], 0.)
            if ur_pose[2] < .08:
                vel[2] = 0.
            if ur_pose[1] > .34:
                print("end of workspace")
                print("log saved: ", logger.save_logs())
                gs.pc.inContact = False
                vel[0] = min(vel[0], 0.)
                vel[1] = 0.

            print("sliding vel ", vel[0], "posx ", pos_x)

            vel = np.array(vel)
            urc.speedl(vel, a=a, t=dt * 2)

            time.sleep(dt)

        else:
            print("no pose estimate")
            print("log saved: ", logger.save_logs())
            break

        # # get tracking image
        # tracking_img = gs.tc.tracking_img
        # if tracking_img is None:
        #     continue

        # slip_index_realtime = gs.tc.slip_index_realtime
        # print("slip_index_realtime", slip_index_realtime)

        # cv2.imshow("marker", tracking_img[:, ::-1])
        # cv2.imshow("diff", gs.tc.diff_raw[:, ::-1] / 255)

        # if urc.getl_rt()[0] < -.45:
        #     break

        # cnt += 1

        c = cv2.waitKey(1) & 0xFF
        if c == ord("q"):
            break

        ##################################################################
        # Record data
        # 'gelsight_url'  : self.gelsight_url,
        # 'fabric_pose'   : self.fabric_pose,
        # 'ur_velocity'   : self.ur_velocity,
        # 'ur_pose'       : self.ur_pose,
        # 'slip_index'    : self.slip_index,
        # 'x'             : self.x,
        # 'y'             : self.x,
        # 'theta'         : self.theta,
        # 'phi'           : self.phi,
        # 'dt'            : self.dt

        if gs.pc.inContact:
            # print("LOGGING")
            # logger.gelsight = gs.pc.diff
            # logger.cable_pose = pose
            logger.ur_velocity = vel
            logger.ur_pose = urc.getl_rt()

            v = np.array([logger.ur_velocity[0], logger.ur_velocity[1]])
            alpha = asin(v[1] / np.sum(v**2)**0.5)

            logger.x = pose[0]
            logger.y = pose[1]
            logger.theta = pose[2]
            # logger.phi = alpha - logger.theta

            logger.dt = time.time() - tm
            tm = time.time()

            logger.add()