import numpy as np
import drivers.devices.kinect_azure.pykinectazure as pk
import visualization.panda.world as wd
import modeling.geometric_model as gm
from vision.depth_camera.calibrator import load_calibration_data
import basis.robot_math as rm
affine_matrix, _, _ = load_calibration_data()
base = wd.World(cam_pos=[10, 2, 7], lookat_pos=[0, 0, 0])
gm.gen_frame().attach_to(base)
pkx = pk.PyKinectAzure()
pcd_list = []
ball_center_list = []
para_list = []
ball_list = []
counter = [0]
def update(pk_obj, pcd_list, ball_center_list, counter, task):
if len(pcd_list) != 0:
for pcd in pcd_list:
pcd.detach()
pcd_list.clear()
# if len(ball_center_list) != 0:
# for ball_center in ball_center_list:
# ball_center.detach()
# ball_center_list.clear()
while True:
pk_obj.device_get_capture()
import pickle
import vision.rgb_camera.util_functions as rgbu
import cv2.aruco as aruco
import numpy as np
import basis.robot_math as rm
origin, origin_rotmat = pickle.load(open("origin.pkl", "rb"))
origin_homomat = np.eye(4)
origin_homomat[:3, :3] = origin_rotmat
origin_homomat[:3, 3] = origin
print(origin, origin_rotmat)
# base = wd.World(cam_pos=origin, lookat_pos=origin_rotmat[:,0]-origin_rotmat[:,1]+origin_rotmat[:,2])
base = wd.World(cam_pos=np.zeros(3), lookat_pos=np.array([0, 0, 10]))
gm.gen_frame(length=1, thickness=.1).attach_to(base)
# base.run()
pk_obj = pk.PyKinectAzure()
pk_obj.device_open()
pk_obj.device_start_cameras()
gm.gen_frame(pos=origin, rotmat=origin_rotmat, length=1, thickness=.03).attach_to(base)
# base.run()
# pcd_list = []
# marker_center_list = []
# def update(pk_obj, pcd_list, marker_center_list, task):
# if len(pcd_list) != 0:
# for pcd in pcd_list:
# pcd.detach()
# pcd_list.clear()
# if len(marker_center_list) != 0:
# for marker_center in marker_center_list:
# marker_center.detach()
# marker_center_list.clear()