From 66a108eeb08a4aa8715e578646e8e04c2a72ff71 Mon Sep 17 00:00:00 2001 From: Ilua Uraev Date: Thu, 5 Jun 2025 21:26:16 +0300 Subject: [PATCH 1/5] add second hand config --- config/gello_config.yaml | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/config/gello_config.yaml b/config/gello_config.yaml index 1203ee1..ef7cb63 100644 --- a/config/gello_config.yaml +++ b/config/gello_config.yaml @@ -4,4 +4,13 @@ usb-FTDI_USB__-__Serial_Converter_FTA7NPA1-if00-port0: joint_signs: [1, 1, 1, 1, 1, -1, 1] gripper: true best_offsets: [6.283, 4.712, 3.142, 3.142, 3.142, 3.142, 4.712] - gripper_range_rad: [2.77856, 3.50931] \ No newline at end of file + gripper_range_rad: [2.77856, 3.50931] + + +usb-FTDI_USB__-__Serial_Converter_FTA7NOIC-if00-port0: + side: "sample-config" + num_joints: 7 + joint_signs: [1, 1, 1, 1, 1, -1, 1] + gripper: true + best_offsets: [6.283, 4.712, 3.142, 3.142, 3.142, 3.142, 4.712] + gripper_range_rad: [2.77856, 3.50931] From 35e38b1443368c3fc88c3f7d0b673f5afb5747bc Mon Sep 17 00:00:00 2001 From: Ilua Uraev Date: Thu, 5 Jun 2025 21:44:49 +0300 Subject: [PATCH 2/5] add offsets of second arm --- config/gello_config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/config/gello_config.yaml b/config/gello_config.yaml index ef7cb63..94fa287 100644 --- a/config/gello_config.yaml +++ b/config/gello_config.yaml @@ -12,5 +12,5 @@ usb-FTDI_USB__-__Serial_Converter_FTA7NOIC-if00-port0: num_joints: 7 joint_signs: [1, 1, 1, 1, 1, -1, 1] gripper: true - best_offsets: [6.283, 4.712, 3.142, 3.142, 3.142, 3.142, 4.712] + best_offsets: [6.283, 0.0, 3.142, 17.142, 4.8, 7.72, 4.112] gripper_range_rad: [2.77856, 3.50931] From cf59c598a0934fc09953cf27c7edaa677aad88aa Mon Sep 17 00:00:00 2001 From: Ilua Uraev Date: Mon, 9 Jun 2025 19:50:35 +0300 Subject: [PATCH 3/5] feat: add use_sim_time parameter --- launch/main.launch.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/launch/main.launch.py b/launch/main.launch.py index 9d6c8d2..4d61d43 100755 --- a/launch/main.launch.py +++ b/launch/main.launch.py @@ -5,17 +5,18 @@ from launch.substitutions import LaunchConfiguration def generate_launch_description(): - args = [] - args.append( - DeclareLaunchArgument(name="com_port", default_value=None, description="Default COM port") - ) + args = [ + DeclareLaunchArgument(name="com_port", default_value=None, description="Default COM port"), + DeclareLaunchArgument(name="use_sim_time", default_value="true", description=""), + ] + nodes = [ Node( package="franka_gello_state_publisher", executable="gello_publisher", name="gello_publisher", output="screen", - parameters=[{"com_port": LaunchConfiguration("com_port")}], + parameters=[{"com_port": LaunchConfiguration("com_port")}, {"use_sim_time": LaunchConfiguration("use_sim_time")}], ), ] From c807df883aec1c2f87dfcd080c4fff89b57345df Mon Sep 17 00:00:00 2001 From: Ilua Uraev Date: Mon, 9 Jun 2025 19:50:49 +0300 Subject: [PATCH 4/5] feat: add get_gello_offsets script --- .../get_gello_offsets.py | 98 +++++++++++++++++++ 1 file changed, 98 insertions(+) create mode 100644 franka_gello_state_publisher/get_gello_offsets.py diff --git a/franka_gello_state_publisher/get_gello_offsets.py b/franka_gello_state_publisher/get_gello_offsets.py new file mode 100644 index 0000000..6269fc3 --- /dev/null +++ b/franka_gello_state_publisher/get_gello_offsets.py @@ -0,0 +1,98 @@ +from dataclasses import dataclass +from pathlib import Path +from typing import Tuple + +import numpy as np +import tyro + +from franka_gello_state_publisher.driver import DynamixelDriver + +MENAGERIE_ROOT: Path = Path(__file__).parent / "third_party" / "mujoco_menagerie" + + +@dataclass +class Args: + port: str = "/dev/ttyUSB0" + """The port that GELLO is connected to.""" + + start_joints: Tuple[float, ...] = (0, 0, 0, 0, 0, 0) + """The joint angles that the GELLO is placed in at (in radians).""" + + joint_signs: Tuple[float, ...] = (1, 1, -1, 1, 1, 1) + """The joint angles that the GELLO is placed in at (in radians).""" + + gripper: bool = True + """Whether or not the gripper is attached.""" + + def __post_init__(self): + assert len(self.joint_signs) == len(self.start_joints) + for idx, j in enumerate(self.joint_signs): + assert ( + j == -1 or j == 1 + ), f"Joint idx: {idx} should be -1 or 1, but got {j}." + + @property + def num_robot_joints(self) -> int: + return len(self.start_joints) + + @property + def num_joints(self) -> int: + extra_joints = 1 if self.gripper else 0 + return self.num_robot_joints + extra_joints + + +def get_config(args: Args) -> None: + joint_ids = list(range(1, args.num_joints)) + driver = DynamixelDriver(joint_ids, port=args.port, baudrate=2000000) + + # assume that the joint state shouold be args.start_joints + # find the offset, which is a multiple of np.pi/2 that minimizes the error between the current joint state and args.start_joints + # this is done by brute force, we seach in a range of +/- 8pi + + def get_error(offset: float, index: int, joint_state: np.ndarray) -> float: + joint_sign_i = args.joint_signs[index] + joint_i = joint_sign_i * (joint_state[index] - offset) + start_i = args.start_joints[index] + return np.abs(joint_i - start_i) + + for _ in range(10): + driver.get_joints() # warmup + + for _ in range(1): + best_offsets = [] + curr_joints = driver.get_joints() + for i in range(args.num_robot_joints): + best_offset = 0 + best_error = 1e6 + for offset in np.linspace( + -8 * np.pi, 8 * np.pi, 8 * 4 + 1 + ): # intervals of pi/2 + error = get_error(offset, i, curr_joints) + if error < best_error: + best_error = error + best_offset = offset + best_offsets.append(best_offset) + print() + print("best offsets : ", [f"{x:.3f}" for x in best_offsets]) + print( + "best offsets function of pi: [" + + ", ".join([f"{int(np.round(x/(np.pi/2)))}*np.pi/2" for x in best_offsets]) + + " ]", + ) + if args.gripper: + print( + "gripper open (degrees) ", + np.rad2deg(driver.get_joints()[-1]) - 0.2, + ) + print( + "gripper close (degrees) ", + np.rad2deg(driver.get_joints()[-1]) - 42, + ) + + +def main(args: Args) -> None: + get_config(args) + + +if __name__ == "__main__": + main(tyro.cli(Args)) From e6a618f78f65bf52ce2153ca27617f8f028a7066 Mon Sep 17 00:00:00 2001 From: Ilua Uraev Date: Mon, 9 Jun 2025 19:51:01 +0300 Subject: [PATCH 5/5] perf: update gello offsets --- config/gello_config.yaml | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/config/gello_config.yaml b/config/gello_config.yaml index 94fa287..66d6076 100644 --- a/config/gello_config.yaml +++ b/config/gello_config.yaml @@ -10,7 +10,10 @@ usb-FTDI_USB__-__Serial_Converter_FTA7NPA1-if00-port0: usb-FTDI_USB__-__Serial_Converter_FTA7NOIC-if00-port0: side: "sample-config" num_joints: 7 - joint_signs: [1, 1, 1, 1, 1, -1, 1] + joint_signs: [1, -1, -1, -1, 1, 1, 1] + gripper: true - best_offsets: [6.283, 0.0, 3.142, 17.142, 4.8, 7.72, 4.112] + # best_offsets: [6.283, 0.0, 3.142, 17.142, 4.8, 7.72, 4.112] + # best_offsets: [6.283, 0.000, 0.000, 4.712, 3.142, 1.571] # 4.712] + best_offsets: [6.283, 0.000, 3.142, 4.712, 3.142, 3.142, 4.102] gripper_range_rad: [2.77856, 3.50931]