Exemple #1
0
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()
Exemple #2
0
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()