60 lines
2.2 KiB
Python
60 lines
2.2 KiB
Python
import numpy as np
|
|
from scipy.spatial.transform import Rotation as R
|
|
import time
|
|
|
|
class ControlUtil:
|
|
|
|
curr_rotation = 0
|
|
|
|
@staticmethod
|
|
def check_limit(new_cam_to_world):
|
|
if new_cam_to_world[0,3] < 0 or new_cam_to_world[1,3] > 0:
|
|
# if new_cam_to_world[0,3] > 0:
|
|
return False
|
|
x = abs(new_cam_to_world[0,3])
|
|
y = abs(new_cam_to_world[1,3])
|
|
tan_y_x = y/x
|
|
min_angle = 0 / 180 * np.pi
|
|
max_angle = 90 / 180 * np.pi
|
|
if tan_y_x < np.tan(min_angle) or tan_y_x > np.tan(max_angle):
|
|
return False
|
|
|
|
return True
|
|
|
|
@staticmethod
|
|
def solve_display_table_rot_and_cam_to_world(cam_to_world: np.ndarray) -> tuple:
|
|
if ControlUtil.check_limit(cam_to_world):
|
|
return 0, cam_to_world
|
|
else:
|
|
min_display_table_rot = 180
|
|
min_new_cam_to_world = None
|
|
for display_table_rot in np.linspace(0.1,360, 1800):
|
|
new_world_to_world = ControlUtil.get_z_axis_rot_mat(display_table_rot)
|
|
new_cam_to_new_world = cam_to_world
|
|
new_cam_to_world = new_world_to_world @ new_cam_to_new_world
|
|
|
|
if ControlUtil.check_limit(new_cam_to_world):
|
|
if display_table_rot < min_display_table_rot:
|
|
min_display_table_rot, min_new_cam_to_world = display_table_rot, new_cam_to_world
|
|
if abs(display_table_rot - 360) < min_display_table_rot:
|
|
min_display_table_rot, min_new_cam_to_world = display_table_rot - 360, new_cam_to_world
|
|
|
|
if min_new_cam_to_world is None:
|
|
raise ValueError("No valid display table rotation found")
|
|
|
|
delta_degree = min_display_table_rot - ControlUtil.curr_rotation
|
|
ControlUtil.curr_rotation = min_display_table_rot
|
|
return delta_degree, min_new_cam_to_world
|
|
|
|
@staticmethod
|
|
def get_z_axis_rot_mat(degree):
|
|
radian = np.radians(degree)
|
|
return np.array([
|
|
[np.cos(radian), -np.sin(radian), 0, 0],
|
|
[np.sin(radian), np.cos(radian), 0, 0],
|
|
[0, 0, 1, 0],
|
|
[0, 0, 0, 1]
|
|
])
|
|
|
|
|