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

finish get_pose and set_pose

Browse Source
This commit is contained in:
hofee 2024-10-06 21:52:05 +08:00
parent 07300696c4
commit 49fdb7f948
1 changed files with 20 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
utils/control_util.py
View File

@ -1,18 +1,19 @@
import numpy as np import numpy as np
from frankapy import FrankaArm from frankapy import FrankaArm
from autolab_core import RigidTransform
class ControlUtil: class ControlUtil:
__fa = FrankaArm(robot_num=2) __fa = FrankaArm(robot_num=2)
DISPLAYTABLE_TO_BASE:np.ndarray = np.asarray([ BASE_TO_DISPLAYTBLE:np.ndarray = np.asarray([
[1, 0, 0, 0], [1, 0, 0, 0],
[0, 1, 0, 0], [0, 1, 0, 0],
[0, 0, 1, 0], [0, 0, 1, 0],
[0, 0, 0, 1] [0, 0, 0, 1]
]) ])
GRIPPER_TO_CAMERA:np.ndarray = np.asarray([ CAMERA_TO_GRIPPER:np.ndarray = np.asarray([
[1, 0, 0, 0], [1, 0, 0, 0],
[0, 1, 0, 0], [0, 1, 0, 0],
[0, 0, 1, 0], [0, 0, 1, 0],
@ -25,15 +26,31 @@ class ControlUtil:
@staticmethod @staticmethod
def get_pose() -> np.ndarray: def get_pose() -> np.ndarray:
return ControlUtil.__fa.get_pose().matrix gripper_to_base = ControlUtil.__fa.get_pose().matrix
cam_to_world = ControlUtil.BASE_TO_DISPLAYTBLE @ gripper_to_base @ ControlUtil.CAMERA_TO_GRIPPER
return cam_to_world
@staticmethod
def set_pose(cam_to_world: np.ndarray) -> None:
gripper_to_base = np.linalg.inv(ControlUtil.BASE_TO_DISPLAYTBLE) @ cam_to_world @ np.linalg.inv(ControlUtil.CAMERA_TO_GRIPPER)
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)
@staticmethod @staticmethod
def reset() -> None: def reset() -> None:
ControlUtil.__fa.reset_joints() ControlUtil.__fa.reset_joints()
# ----------- Debug Test ------------- # ----------- Debug Test -------------
if __name__ == "__main__": if __name__ == "__main__":
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()) print(ControlUtil.get_pose())
ControlUtil.reset() ControlUtil.reset()
print(ControlUtil.get_pose()) print(ControlUtil.get_pose())