Fix undefined reference due to protobuf version error

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"
     )

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

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:

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):