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()