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

update displaytable rotation

Browse Source
This commit is contained in:
hofee 2024-10-14 19:37:34 +08:00
parent 2f87a2626c
commit 20514be419
1 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
utils/control_util.py
View File

@ -8,7 +8,7 @@ class ControlUtil:
__fa:FrankaArm = None __fa:FrankaArm = None
__ser: serial.Serial = None __ser: serial.Serial = None
cnt_rotation = 0 curr_rotation = 0
BASE_TO_WORLD:np.ndarray = np.asarray([ BASE_TO_WORLD:np.ndarray = np.asarray([
[1, 0, 0, -0.61091665], [1, 0, 0, -0.61091665],
@ -64,13 +64,14 @@ class ControlUtil:
"left": 1, "left": 1,
"right": 0 "right": 0
} }
ControlUtil.cnt_rotation += degree delta_degree = degree - ControlUtil.curr_rotation
ControlUtil.curr_rotation += delta_degree
print(f"Table rotated {ControlUtil.cnt_rotation} degree") print(f"Table rotated {ControlUtil.cnt_rotation} degree")
if degree >= 0: if degree >= 0:
turn_angle = degree turn_angle = delta_degree
turn_direction = turn_directions["right"] turn_direction = turn_directions["right"]
else: else:
turn_angle = -degree turn_angle = -delta_degree
turn_direction = turn_directions["left"] turn_direction = turn_directions["left"]
write_len = ControlUtil.__ser.write(f"CT+TRUNSINGLE({turn_direction},{turn_angle});".encode('utf-8')) write_len = ControlUtil.__ser.write(f"CT+TRUNSINGLE({turn_direction},{turn_angle});".encode('utf-8'))