From c342fd946a5ad58933d8f003cfe998edce1ca7d5 Mon Sep 17 00:00:00 2001 From: Jeremy Morgan Date: Tue, 29 Jul 2025 13:38:33 -0700 Subject: [PATCH 1/4] fix `undefined reference` due to protobuf version error --- deoxys/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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" ) From b951b096dfd268d3fded642879f90c2bc4b3e3a3 Mon Sep 17 00:00:00 2001 From: Jeremy Morgan Date: Tue, 12 Aug 2025 14:21:06 -0700 Subject: [PATCH 2/4] Added support for product_id=50741 --- deoxys/deoxys/utils/io_devices/spacemouse.py | 128 +++++++++++++------ 1 file changed, 92 insertions(+), 36 deletions(-) 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): From 4bc62c8f7ae58917701f490de1de61508dbe40f8 Mon Sep 17 00:00:00 2001 From: Jeremy Morgan Date: Thu, 21 Aug 2025 14:03:39 -0700 Subject: [PATCH 3/4] Adding last_qd_d, last_d_joint_torques, last_qd_d, last_qdd_d to franka_interface.py --- .../franka_interface/franka_interface.py | 37 +++++++++++++++++++ 1 file changed, 37 insertions(+) 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: From 5537bca5cf1630064a101cde0035553fe80dc1d3 Mon Sep 17 00:00:00 2001 From: Jeremy Morgan Date: Tue, 26 Aug 2025 12:13:18 -0700 Subject: [PATCH 4/4] Update charmander.yml to RESL/SLURM specific ips --- deoxys/config/charmander.yml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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