robossembler/gello-publisher
7
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

add joint offsets

Browse source
This commit is contained in:
Ilya Uraev 2025-06-04 17:40:04 +03:00
parent f2204f3cb4
commit 9f97820710
2 changed files with 13 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

17
franka_gello_state_publisher/driver.py
View file

@ -92,7 +92,11 @@ class FakeDynamixelDriver(DynamixelDriverProtocol):
class DynamixelDriver(DynamixelDriverProtocol): class DynamixelDriver(DynamixelDriverProtocol):
def __init__( 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. """Initialize the DynamixelDriver class.
@ -120,6 +124,7 @@ class DynamixelDriver(DynamixelDriverProtocol):
ADDR_GOAL_POSITION, ADDR_GOAL_POSITION,
LEN_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 # Open the port and set the baudrate
if not self._portHandler.openPort(): if not self._portHandler.openPort():
@ -153,9 +158,10 @@ class DynamixelDriver(DynamixelDriverProtocol):
if not self._torque_enabled: if not self._torque_enabled:
raise RuntimeError("Torque must be enabled to set joint angles") 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 # 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 # Allocate goal position value into byte array
param_goal_position = [ param_goal_position = [
@ -237,8 +243,9 @@ class DynamixelDriver(DynamixelDriverProtocol):
while self._joint_angles is None: while self._joint_angles is None:
time.sleep(0.1) time.sleep(0.1)
# with self._lock: # with self._lock:
_j = self._joint_angles.copy() raw_angles = self._joint_angles.copy()
return _j / 2048.0 * np.pi angles = raw_angles / 2048.0 * np.pi
return angles - self._offsets
def close(self): def close(self):
self._stop_thread.set() self._stop_thread.set()

2
franka_gello_state_publisher/gello_publisher.py
View file

@ -69,7 +69,7 @@ class GelloPublisher(Node):
"""Whether or not the gripper is attached.""" """Whether or not the gripper is attached."""
joint_ids = list(range(1, self.num_joints)) 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.""" """The driver for the Dynamixel motors."""
self.best_offsets = np.array(config[self.port]["best_offsets"]) self.best_offsets = np.array(config[self.port]["best_offsets"])