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-10 21:48:55 +08:00
parent 8fd2d6b1e1
commit cd85fed3a0
10 changed files with 73 additions and 33004 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
utils/control_util.py
View File

@@ -19,7 +19,7 @@ class ControlUtil:
CAMERA_TO_GRIPPER:np.ndarray = np.asarray([
[0, -1, 0, 0.01],
[1, 0, 0, -0.05],
[1, 0, 0, 0],
[0, 0, 1, 0.075],
[0, 0, 0, 1]
])
@@ -33,8 +33,10 @@ class ControlUtil:
@staticmethod
def connect_robot():
ControlUtil.__ser = serial.Serial(port="/dev/ttyUSB0", baudrate=115200)
ControlUtil.__fa = FrankaArm(robot_num=2)
if ControlUtil.__fa is None:
ControlUtil.__fa = FrankaArm(robot_num=2)
if ControlUtil.__ser is None:
ControlUtil.__ser = serial.Serial(port="/dev/ttyUSB0", baudrate=115200)
@staticmethod
def franka_reset() -> None:
@@ -63,8 +65,7 @@ class ControlUtil:
"right": 1
}
ControlUtil.cnt_rotation += degree
print("total rot:", ControlUtil.cnt_rotation)
print(f"Table rotated {ControlUtil.cnt_rotation} degree")
if degree >= 0:
turn_angle = degree
turn_direction = turn_directions["right"]
@@ -146,7 +147,6 @@ class ControlUtil:
@staticmethod
def move_to(pose: np.ndarray):
rot_degree, cam_to_world = ControlUtil.solve_display_table_rot_and_cam_to_world(pose)
print("table rot degree:", rot_degree)
exec_time = rot_degree/9
start_time = time.time()
ControlUtil.rotate_display_table(rot_degree)
@@ -154,25 +154,17 @@ class ControlUtil:
end_time = time.time()
if end_time - start_time < exec_time:
time.sleep(exec_time - (end_time - start_time))
# ----------- Debug Test -------------
if __name__ == "__main__":
ControlUtil.connect_robot()
print(ControlUtil.get_curr_gripper_to_base_pose())
ControlUtil.init()
# import time
# start = time.time()
# test_pose = np.asarray([[ 0.06023737 ,-0.81328565, 0.57872715, -0.23602393],
# [-0.99107872 ,-0.11773834, -0.06230158, 0.24174289],
# [ 0.11880735 ,-0.56981127 ,-0.813138 , 0.15199069],
# [ 0. , 0. , 0. , 1. , ]])
# print(test_pose)
# rot_degree, cam_to_world = ControlUtil.solve_display_table_rot_and_cam_to_world(test_pose)
# print(rot_degree, cam_to_world)
# end = time.time()
# print(f"Time: {end-start}")
# ControlUtil.set_pose(test_pose)
# import ipdb; ipdb.set_trace()
# ControlUtil.move_to(test_pose)
# print("End!")
def main_test():
print(ControlUtil.get_curr_gripper_to_base_pose())
ControlUtil.init()
def rotate_back(rotation):
ControlUtil.rotate_display_table(-rotation)
rotate_back(122.0661478599)