hofee/nbv_rec_control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
hofee
2024-10-12 20:25:55 +08:00
parent 3fe74eb6eb
commit 8d43d4de60
3 changed files with 198 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files

69
utils/control_util.py
View File

@@ -61,8 +61,8 @@ class ControlUtil:
@staticmethod
def rotate_display_table(degree):
turn_directions = {
"left": 0,
"right": 1
"left": 1,
"right": 0
}
ControlUtil.cnt_rotation += degree
print(f"Table rotated {ControlUtil.cnt_rotation} degree")
@@ -114,7 +114,7 @@ class ControlUtil:
min_new_cam_to_world = None
for display_table_rot in np.linspace(0.1,360, 1800):
display_table_rot_z_pose = ControlUtil.get_z_axis_rot_mat(display_table_rot)
new_cam_to_world = display_table_rot_z_pose @ cam_to_world
new_cam_to_world = np.linalg.inv(display_table_rot_z_pose) @ cam_to_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
@@ -147,11 +147,12 @@ class ControlUtil:
@staticmethod
def move_to(pose: np.ndarray):
rot_degree, cam_to_world = ControlUtil.solve_display_table_rot_and_cam_to_world(pose)
exec_time = rot_degree/9
exec_time = abs(rot_degree)/9
start_time = time.time()
ControlUtil.rotate_display_table(rot_degree)
ControlUtil.set_pose(cam_to_world)
end_time = time.time()
print(f"Move to pose with rotation {rot_degree} degree, exec time: {end_time - start_time}|exec time: {exec_time}")
if end_time - start_time < exec_time:
time.sleep(exec_time - (end_time - start_time))
@@ -159,7 +160,7 @@ class ControlUtil:
if __name__ == "__main__":
ControlUtil.connect_robot()
#ControlUtil.franka_reset()
# ControlUtil.franka_reset()
def main_test():
print(ControlUtil.get_curr_gripper_to_base_pose())
ControlUtil.init()
@@ -167,7 +168,59 @@ if __name__ == "__main__":
def rotate_back(rotation):
ControlUtil.rotate_display_table(-rotation)
#rotate_back(45.3125623)
#main_test()
import sys; sys.path.append("/home/yan20/nbv_rec/project/franka_control")
from utils.communicate_util import CommunicateUtil
import ipdb
ControlUtil.init()
#print(ControlUtil.get_curr_gripper_to_base_pose())
view_data_0 = CommunicateUtil.get_view_data(init=True)
depth_extrinsics_0 = view_data_0["depth_extrinsics"]
cam_to_world_0 = ControlUtil.get_pose()
print("cam_extrinsics_0")
print(depth_extrinsics_0)
print("cam_to_world_0")
print(cam_to_world_0)
ipdb.set_trace()
TEST_POSE:np.ndarray = np.asarray([
[ 0.46532393, 0.62171798, 0.63002284, 0.30230963],
[ 0.43205618, -0.78075723, 0.45136491, -0.29127173],
[ 0.77251656, 0.06217437, -0.63193429, 0.559957 ],
[ 0. , 0. , 0. , 1. ],
])
TEST_POSE_CAM_TO_WORLD = ControlUtil.BASE_TO_WORLD @ TEST_POSE @ ControlUtil.CAMERA_TO_GRIPPER
ControlUtil.move_to(TEST_POSE_CAM_TO_WORLD)
view_data_1 = CommunicateUtil.get_view_data()
depth_extrinsics_1 = view_data_1["depth_extrinsics"]
depth_extrinsics_1[:3,3] = depth_extrinsics_1[:3,3] / 1000
cam_to_world_1 = ControlUtil.get_pose()
print("cam_extrinsics_1")
print(depth_extrinsics_1)
print("cam_to_world_1")
print(TEST_POSE_CAM_TO_WORLD)
actual_cam_to_world_1 = cam_to_world_0 @ depth_extrinsics_1
print("actual_cam_to_world_1")
print(actual_cam_to_world_1)
ipdb.set_trace()
TEST_POSE_2:np.ndarray = np.asarray(
[[ 0.74398544, -0.61922696, 0.251049, 0.47000935],
[-0.47287207, -0.75338888, -0.45692666, 0.20843903],
[ 0.47207883 , 0.22123272, -0.85334192, 0.57863381],
[ 0. , 0. , 0. , 1. , ]]
)
TEST_POSE_CAM_TO_WORLD_2 = ControlUtil.BASE_TO_WORLD @ TEST_POSE_2 @ ControlUtil.CAMERA_TO_GRIPPER
#ControlUtil.move_to(TEST_POSE_CAM_TO_WORLD_2)
ControlUtil.set_pose(TEST_POSE_CAM_TO_WORLD_2)
view_data_2 = CommunicateUtil.get_view_data()
depth_extrinsics_2 = view_data_2["depth_extrinsics"]
depth_extrinsics_2[:3,3] = depth_extrinsics_2[:3,3] / 1000
cam_to_world_2 = ControlUtil.get_pose()
print("cam_extrinsics_2")
print(depth_extrinsics_2)
print("cam_to_world_2")
print(TEST_POSE_CAM_TO_WORLD_2)
actual_cam_to_world_2 = cam_to_world_0 @ depth_extrinsics_2
print("actual_cam_to_world_2")
print(actual_cam_to_world_2)
ipdb.set_trace()