add joint offsets

franka_gello_state_publisher/driver.py

@@ -92,7 +92,11 @@ class FakeDynamixelDriver(DynamixelDriverProtocol):
 
 class DynamixelDriver(DynamixelDriverProtocol):
     def __init__(
-        self, ids: Sequence[int], port: str = "/dev/ttyUSB0", baudrate: int = 2000000
+        self,
+        ids: Sequence[int],
+        port: str = "/dev/ttyUSB0",
+        baudrate: int = 2000000,
+        offsets: Sequence[float] | None = None,
     ):
         """Initialize the DynamixelDriver class.
 
@@ -120,6 +124,7 @@ class DynamixelDriver(DynamixelDriverProtocol):
             ADDR_GOAL_POSITION,
             LEN_GOAL_POSITION,
         )
+        self._offsets = np.array(offsets if offsets is not None else [0.0] * len(ids))
 
         # Open the port and set the baudrate
         if not self._portHandler.openPort():
@@ -153,9 +158,10 @@ class DynamixelDriver(DynamixelDriverProtocol):
         if not self._torque_enabled:
             raise RuntimeError("Torque must be enabled to set joint angles")
 
-        for dxl_id, angle in zip(self._ids, joint_angles):
+        for dxl_id, angle, offsets in zip(self._ids, joint_angles, self._offsets):
             # Convert the angle to the appropriate value for the servo
-            position_value = int(angle * 2048 / np.pi)
+            angle_with_offset = angle + offsets
+            position_value = int(angle_with_offset * 2048 / np.pi)
 
             # Allocate goal position value into byte array
             param_goal_position = [
@@ -237,8 +243,9 @@ class DynamixelDriver(DynamixelDriverProtocol):
         while self._joint_angles is None:
             time.sleep(0.1)
         # with self._lock:
-        _j = self._joint_angles.copy()
-        return _j / 2048.0 * np.pi
+        raw_angles = self._joint_angles.copy()
+        angles = raw_angles / 2048.0 * np.pi
+        return angles - self._offsets
 
     def close(self):
         self._stop_thread.set()

franka_gello_state_publisher/gello_publisher.py

@@ -69,7 +69,7 @@ class GelloPublisher(Node):
         """Whether or not the gripper is attached."""
 
         joint_ids = list(range(1, self.num_joints))
-        self.driver = DynamixelDriver(joint_ids, port=self.com_port, baudrate=2000000)
+        self.driver = DynamixelDriver(joint_ids, port=self.com_port, baudrate=2000000, offsets=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
         """The driver for the Dynamixel motors."""
 
         self.best_offsets = np.array(config[self.port]["best_offsets"])