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

finish pipeline

Browse Source
This commit is contained in:
hofee
2024-10-09 21:46:13 +08:00
parent f8514564c1
commit ba36803fba
8 changed files with 325 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files

151
utils/control_util.py
View File

@@ -1,41 +1,49 @@
import numpy as np
from frankapy import FrankaArm
from autolab_core import RigidTransform
import serial
import time
class ControlUtil:
#__fa = FrankaArm(robot_num=2)
__fa:FrankaArm = None
__ser: serial.Serial = None
cnt_rotation = 0
BASE_TO_WORLD:np.ndarray = np.asarray([
[1, 0, 0, -0.5],
[0, 1, 0, 0],
[0, 0, 1, -0.2],
[1, 0, 0, -0.7323],
[0, 1, 0, 0.05926],
[0, 0, 1, -0.21],
[0, 0, 0, 1]
])
CAMERA_TO_GRIPPER:np.ndarray = np.asarray([
[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, -1, 0, 0.01],
[1, 0, 0, -0.05],
[0, 0, 1, 0.075],
[0, 0, 0, 1]
])
theta = np.radians(25)
INIT_POSE:np.ndarray = np.asarray([
[np.cos(theta), 0, -np.sin(theta), 0],
[0, -1, 0, 0],
[-np.sin(theta), 0, -np.cos(theta), 0.35],
[0, 0, 0, 1]
INIT_GRIPPER_POSE:np.ndarray = np.asarray([
[ 0.44808722 , 0.61103352 , 0.65256787 , 0.36428118],
[ 0.51676868 , -0.77267257 , 0.36866054, -0.26519364],
[ 0.72948524 , 0.17203456 ,-0.66200043 , 0.60938969],
[ 0. , 0. , 0. , 1. ]
])
AXIS_THRESHOLD = (-(np.pi+5e-2)/2, (np.pi+5e-2)/2)
@staticmethod
def connect_robot():
ControlUtil.__ser = serial.Serial(port="/dev/ttyUSB0", baudrate=115200)
ControlUtil.__fa = FrankaArm(robot_num=2)
@staticmethod
def franka_reset() -> None:
ControlUtil.__fa.reset_joints()
@staticmethod
def init() -> None:
ControlUtil.set_pose(ControlUtil.INIT_POSE)
ControlUtil.franka_reset()
ControlUtil.set_gripper_pose(ControlUtil.INIT_GRIPPER_POSE)
@staticmethod
def get_pose() -> np.ndarray:
@@ -46,17 +54,35 @@ class ControlUtil:
@staticmethod
def set_pose(cam_to_world: np.ndarray) -> None:
gripper_to_base = ControlUtil.solve_gripper_to_base(cam_to_world)
gripper_to_base = RigidTransform(rotation=gripper_to_base[:3, :3], translation=gripper_to_base[:3, 3], from_frame="franka_tool", to_frame="world")
ControlUtil.__fa.goto_pose(gripper_to_base, use_impedance=False, ignore_errors=False)
ControlUtil.set_gripper_pose(gripper_to_base)
@staticmethod
def rotate_display_table(degree):
pass
turn_directions = {
"left": 0,
"right": 1
}
ControlUtil.cnt_rotation += degree
print("total rot:", ControlUtil.cnt_rotation)
if degree >= 0:
turn_angle = degree
turn_direction = turn_directions["right"]
else:
turn_angle = -degree
turn_direction = turn_directions["left"]
write_len = ControlUtil.__ser.write(f"CT+TRUNSINGLE({turn_direction},{turn_angle});".encode('utf-8'))
@staticmethod
def get_curr_gripper_to_base_pose() -> np.ndarray:
return ControlUtil.__fa.get_pose().matrix
@staticmethod
def set_gripper_pose(gripper_to_base: np.ndarray) -> None:
gripper_to_base = RigidTransform(rotation=gripper_to_base[:3, :3], translation=gripper_to_base[:3, 3], from_frame="franka_tool", to_frame="world")
ControlUtil.__fa.goto_pose(gripper_to_base, duration=5, use_impedance=False, ignore_errors=False)
@staticmethod
def solve_gripper_to_base(cam_to_world: np.ndarray) -> np.ndarray:
return np.linalg.inv(ControlUtil.BASE_TO_WORLD) @ cam_to_world @ np.linalg.inv(ControlUtil.CAMERA_TO_GRIPPER)
@@ -66,24 +92,38 @@ class ControlUtil:
return ControlUtil.BASE_TO_WORLD @ gripper_to_base @ ControlUtil.CAMERA_TO_GRIPPER
@staticmethod
def solve_display_table_rot_and_cam_to_world(cam_to_world: np.ndarray) -> tuple:
gripper_to_base = ControlUtil.solve_gripper_to_base(cam_to_world)
gripper_to_base_axis_angle = ControlUtil.get_gripper_to_base_axis_angle(gripper_to_base)
def check_limit(new_cam_to_world):
if new_cam_to_world[0,3] > 0 or new_cam_to_world[1,3] > 0:
return False
x = abs(new_cam_to_world[0,3])
y = abs(new_cam_to_world[1,3])
if ControlUtil.AXIS_THRESHOLD[0] <= gripper_to_base_axis_angle <= ControlUtil.AXIS_THRESHOLD[1]:
tan_y_x = y/x
if tan_y_x < np.sqrt(3)/3 or tan_y_x > np.sqrt(3):
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:
for display_table_rot in np.linspace(0.1,180, 1800):
min_display_table_rot = 180
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
if ControlUtil.AXIS_THRESHOLD[0] <= ControlUtil.get_gripper_to_base_axis_angle(new_cam_to_world) <= ControlUtil.AXIS_THRESHOLD[1]:
return -display_table_rot, new_cam_to_world
display_table_rot = -display_table_rot
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
if ControlUtil.AXIS_THRESHOLD[0] <= ControlUtil.get_gripper_to_base_axis_angle(new_cam_to_world) <= ControlUtil.AXIS_THRESHOLD[1]:
return -display_table_rot, new_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
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")
return min_display_table_rot, min_new_cam_to_world
@staticmethod
def get_z_axis_rot_mat(degree):
@@ -107,35 +147,32 @@ class ControlUtil:
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)
ControlUtil.set_pose(cam_to_world)
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()
rot_degree, cam_to_world = ControlUtil.solve_display_table_rot_and_cam_to_world(ControlUtil.INIT_POSE)
end = time.time()
print(f"Time: {end-start}")
print(rot_degree, cam_to_world)
# test_pose = np.asarray([
# [1, 0, 0, 0.4],
# [0, -1, 0, 0],
# [0, 0, -1, 0.6],
# [0, 0, 0, 1]
# ])
# ControlUtil.set_pose(test_pose)
# print(ControlUtil.get_pose())
# ControlUtil.reset()
# print(ControlUtil.get_pose())
# 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}")
angle = ControlUtil.get_gripper_to_base_axis_angle(ControlUtil.solve_gripper_to_base(cam_to_world))
threshold = ControlUtil.AXIS_THRESHOLD
angle_degree = np.degrees(angle)
threshold_degree = np.degrees(threshold[0]), np.degrees(threshold[1])
print(f"Angle: {angle_degree}, range: {threshold_degree}")
ControlUtil.set_pose(cam_to_world)
# ControlUtil.set_pose(test_pose)
# import ipdb; ipdb.set_trace()
# ControlUtil.move_to(test_pose)
# print("End!")