diff --git a/deoxys/CMakeLists.txt b/deoxys/CMakeLists.txt index aaf0eaa..3fcfccb 100644 --- a/deoxys/CMakeLists.txt +++ b/deoxys/CMakeLists.txt @@ -56,7 +56,7 @@ if (BUILD_FRANKA) add_proto_cc_library(shared_protobuf NAMESPACE ${PROTO_NAMESPACE} PROTO_FILES ${FRANKA_PROTO_FILES} - PROTO_LIB protobuf) + PROTO_LIB protobuf::libprotobuf) set(BUILD_TESTS OFF CACHE BOOL "Turn off libfranka tests") set(BUILD_EXAMPLES OFF CACHE BOOL "Turn off libfranka examples") find_package(Eigen3 REQUIRED) @@ -141,7 +141,7 @@ endif(BUILD_FRANKA) if (BUILD_DEOXYS) add_proto_py_library(py_protobuf PROTO_FILES ${PROTO_FILES} - PROTO_LIB protobuf) + PROTO_LIB protobuf::libprotobuf) install(DIRECTORY "${PY_PROTOBUF_OUTPUT_DIR}/" DESTINATION "${PYTHON_MODULE_OUTPUT_DIRECTORY}/deoxys/proto" ) diff --git a/deoxys/config/charmander.yml b/deoxys/config/charmander.yml index 5945fd0..81abedb 100644 --- a/deoxys/config/charmander.yml +++ b/deoxys/config/charmander.yml @@ -1,10 +1,10 @@ PC: - NAME: "rosie" - IP: 172.16.0.1 + NAME: "hanyang-coldbrewTMP" + IP: 172.16.0.5 NUC: - NAME: "charmander" - IP: 172.16.0.3 + NAME: "baskin3" + IP: 172.16.0.1 PUB_PORT: 5556 SUB_PORT: 5555 GRIPPER_PUB_PORT: 5558 diff --git a/deoxys/deoxys/franka_interface/franka_interface.py b/deoxys/deoxys/franka_interface/franka_interface.py index 6613acb..bc52bf9 100644 --- a/deoxys/deoxys/franka_interface/franka_interface.py +++ b/deoxys/deoxys/franka_interface/franka_interface.py @@ -16,6 +16,8 @@ logger = logging.getLogger(__name__) +print(f"{franka_robot_state_pb2.FrankaRobotStateMessage=}") +print() def action_to_osc_pose_goal(action, is_delta=True) -> franka_controller_pb2.Goal: goal = franka_controller_pb2.Goal() @@ -537,6 +539,29 @@ def last_eef_pose(self) -> np.ndarray: return None return np.array(self._state_buffer[-1].O_T_EE).reshape(4, 4).transpose() + @property + def last_joint_torques(self) -> np.ndarray: + """_summary_ + + Returns: + np.ndarray: The 4x4 homogeneous matrix of end effector pose. + """ + if self.state_buffer_size == 0: + return None + return np.array(self._state_buffer[-1].tau_J) + + @property + def last_d_joint_torques(self) -> np.ndarray: + """_summary_ + + Returns: + np.ndarray: The 4x4 homogeneous matrix of end effector pose. + """ + if self.state_buffer_size == 0: + return None + return np.array(self._state_buffer[-1].dtau_J) + + @property def last_eef_rot_and_pos(self) -> Tuple[np.ndarray, np.ndarray]: """_summary_ @@ -604,6 +629,18 @@ def last_q_d(self) -> np.ndarray: return None return np.array(self._state_buffer[-1].q_d) + @property + def last_qd_d(self) -> np.ndarray: + if self.state_buffer_size == 0: + return None + return np.array(self._state_buffer[-1].dq_d) + + @property + def last_qdd_d(self) -> np.ndarray: + if self.state_buffer_size == 0: + return None + return np.array(self._state_buffer[-1].ddq_d) + @property def last_gripper_q(self) -> np.ndarray: if self.gripper_state_buffer_size == 0: diff --git a/deoxys/deoxys/utils/io_devices/spacemouse.py b/deoxys/deoxys/utils/io_devices/spacemouse.py index bd88e24..5d1182c 100644 --- a/deoxys/deoxys/utils/io_devices/spacemouse.py +++ b/deoxys/deoxys/utils/io_devices/spacemouse.py @@ -19,6 +19,8 @@ import threading import time from collections import namedtuple +from time import sleep +from termcolor import cprint import numpy as np @@ -113,6 +115,8 @@ def __init__( self, vendor_id=9583, product_id=50735, pos_sensitivity=1.0, rot_sensitivity=1.0 ): + self.product_id = product_id + print("Opening SpaceMouse device") self.device = hid.device() self.device.open(vendor_id, product_id) # SpaceMouse @@ -217,44 +221,96 @@ def run(self): while True: d = self.device.read(13) + # try: + # d = self.device.read(13) + # except OSError as e: + # cprint(f"Error reading from SpaceMouse: {e}", "red") + # sleep(0.0001) + # continue + if d is not None and self._enabled: - if d[0] == 1: ## readings from 6-DoF sensor - self.y = convert(d[1], d[2]) - self.x = convert(d[3], d[4]) - self.z = convert(d[5], d[6]) * -1.0 - - self.roll = convert(d[7], d[8]) - self.pitch = convert(d[9], d[10]) - self.yaw = convert(d[11], d[12]) - - self._control = [ - self.x, - self.y, - self.z, - self.roll, - self.pitch, - self.yaw, - ] - - elif d[0] == 3: ## readings from the side buttons - - # press left button - if d[1] == 1: - t_click = time.time() - elapsed_time = t_click - t_last_click - t_last_click = t_click - self.single_click_and_hold = True - - # release left button - if d[1] == 0: - self.single_click_and_hold = False - - # right button is for reset - if d[1] == 2: - self._reset_state = 1 - self._enabled = False - self._reset_internal_state() + + if self.product_id == 50741: + ## logic for older spacemouse model + + if d[0] == 1: ## readings from 6-DoF sensor + self.y = convert(d[1], d[2]) + self.x = convert(d[3], d[4]) + self.z = convert(d[5], d[6]) * -1.0 + + elif d[0] == 2: + + self.roll = convert(d[1], d[2]) + self.pitch = convert(d[3], d[4]) + self.yaw = convert(d[5], d[6]) + + self._control = [ + self.x, + self.y, + self.z, + self.roll, + self.pitch, + self.yaw, + ] + elif d[0] == 3: ## readings from the side buttons + + # press left button + if d[1] == 1: + t_click = time.time() + elapsed_time = t_click - t_last_click + t_last_click = t_click + self.single_click_and_hold = True + + # release left button + if d[1] == 0: + self.single_click_and_hold = False + + # right button is for reset + if d[1] == 2: + self._reset_state = 1 + self._enabled = False + self._reset_internal_state() + + else: + ## default logic for all other spacemouse models + + if d[0] == 1: ## readings from 6-DoF sensor + self.y = convert(d[1], d[2]) + self.x = convert(d[3], d[4]) + self.z = convert(d[5], d[6]) * -1.0 + + self.roll = convert(d[7], d[8]) + self.pitch = convert(d[9], d[10]) + self.yaw = convert(d[11], d[12]) + + self._control = [ + self.x, + self.y, + self.z, + self.roll, + self.pitch, + self.yaw, + ] + + elif d[0] == 3: ## readings from the side buttons + + # press left button + if d[1] == 1: + t_click = time.time() + elapsed_time = t_click - t_last_click + t_last_click = t_click + self.single_click_and_hold = True + + # release left button + if d[1] == 0: + self.single_click_and_hold = False + + # right button is for reset + if d[1] == 2: + self._reset_state = 1 + self._enabled = False + self._reset_internal_state() @property def control(self):