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 16:39:00 +08:00
parent cd85fed3a0
commit 3fe74eb6eb
5 changed files with 226 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
utils/control_util.py
View File

@@ -11,23 +11,23 @@ class ControlUtil:
cnt_rotation = 0
BASE_TO_WORLD:np.ndarray = np.asarray([
[1, 0, 0, -0.7323],
[0, 1, 0, 0.05926],
[0, 0, 1, -0.21],
[1, 0, 0, -0.61091665],
[0, 1, 0, -0.00309726],
[0, 0, 1, -0.1136743],
[0, 0, 0, 1]
])
CAMERA_TO_GRIPPER:np.ndarray = np.asarray([
[0, -1, 0, 0.01],
[1, 0, 0, 0],
[0, 0, 1, 0.075],
[0, 0, 1, 0.08],
[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. ]
[ 0.46532393, 0.62171798, 0.63002284, 0.21230963],
[ 0.43205618, -0.78075723, 0.45136491, -0.25127173],
[ 0.77251656, 0.06217437, -0.63193429, 0.499957 ],
[ 0. , 0. , 0. , 1. ],
])
@@ -159,7 +159,7 @@ class ControlUtil:
if __name__ == "__main__":
ControlUtil.connect_robot()
#ControlUtil.franka_reset()
def main_test():
print(ControlUtil.get_curr_gripper_to_base_pose())
ControlUtil.init()
@@ -167,4 +167,7 @@ if __name__ == "__main__":
def rotate_back(rotation):
ControlUtil.rotate_display_table(-rotation)
rotate_back(122.0661478599)
#rotate_back(45.3125623)
ControlUtil.init()
#print(ControlUtil.get_curr_gripper_to_base_pose())