39 lines
870 B
Python
39 lines
870 B
Python
import numpy as np
|
|
from frankapy import FrankaArm
|
|
|
|
class ControlUtil:
|
|
|
|
__fa = FrankaArm(robot_num=2)
|
|
|
|
DISPLAYTABLE_TO_BASE:np.ndarray = np.asarray([
|
|
[1, 0, 0, 0],
|
|
[0, 1, 0, 0],
|
|
[0, 0, 1, 0],
|
|
[0, 0, 0, 1]
|
|
])
|
|
|
|
GRIPPER_TO_CAMERA:np.ndarray = np.asarray([
|
|
[1, 0, 0, 0],
|
|
[0, 1, 0, 0],
|
|
[0, 0, 1, 0],
|
|
[0, 0, 0, 1]
|
|
])
|
|
|
|
@staticmethod
|
|
def get_franka_arm() -> FrankaArm:
|
|
return ControlUtil.__fa
|
|
|
|
@staticmethod
|
|
def get_pose() -> np.ndarray:
|
|
return ControlUtil.__fa.get_pose().matrix
|
|
|
|
@staticmethod
|
|
def reset() -> None:
|
|
ControlUtil.__fa.reset_joints()
|
|
|
|
# ----------- Debug Test -------------
|
|
|
|
if __name__ == "__main__":
|
|
print(ControlUtil.get_pose())
|
|
ControlUtil.reset()
|
|
print(ControlUtil.get_pose()) |