From 8e84ad0222b38a83bccc52f694687db31340db53 Mon Sep 17 00:00:00 2001 From: Yuwei Xia Date: Mon, 23 Sep 2024 13:19:29 -0700 Subject: [PATCH 1/7] UPDATE emancro gello --- BUILD.bazel | 17 +++++ experiments/run_env.py | 9 ++- gello/agents/gello_agent.py | 128 +++++++++++++++++++----------------- gello/dynamixel/driver.py | 50 +++++++++++++- gello/robots/dynamixel.py | 6 ++ installdeps.sh | 7 ++ 6 files changed, 153 insertions(+), 64 deletions(-) create mode 100644 BUILD.bazel create mode 100755 installdeps.sh diff --git a/BUILD.bazel b/BUILD.bazel new file mode 100644 index 0000000..b37d1b9 --- /dev/null +++ b/BUILD.bazel @@ -0,0 +1,17 @@ + +load("@rules_python//python:defs.bzl", "py_library", "py_binary") +load("@python_deps//:requirements.bzl", "requirement") + +py_library( + name = "gello", + srcs = glob(["*.py", "**/*.py"]), + deps = [ + requirement("numpy"), + requirement("torchvision"), + requirement("torch"), + requirement("ultralytics"), + requirement("opencv-python"), + requirement("pyquaternion"), + ], + visibility = ["//visibility:public"], +) diff --git a/experiments/run_env.py b/experiments/run_env.py index bd2319a..20aaf97 100644 --- a/experiments/run_env.py +++ b/experiments/run_env.py @@ -117,7 +117,7 @@ def main(args): ) if args.start_joints is None: reset_joints = np.deg2rad( - [0, -90, 90, -90, -90, 0, 0] + [90, 0, 0, 0, 0, 0, 0, 0] ) # Change this to your own reset joints else: reset_joints = args.start_joints @@ -146,10 +146,12 @@ def main(args): raise ValueError("Invalid agent name") # going to start position - print("Going to start position") + print(f"Going to start position {env.get_obs()}") start_pos = agent.act(env.get_obs()) + print(f"start_pos as: {start_pos}" ) obs = env.get_obs() joints = obs["joint_positions"] + print(f"joints {joints}") abs_deltas = np.abs(start_pos - joints) id_max_joint_delta = np.argmax(abs_deltas) @@ -189,7 +191,7 @@ def main(args): obs = env.get_obs() joints = obs["joint_positions"] action = agent.act(obs) - if (action - joints > 0.5).any(): + if (action - joints > 0.5*10).any(): print("Action is too big") # print which joints are too big @@ -220,6 +222,7 @@ def main(args): flush=True, ) action = agent.act(obs) + print(f"action: {action}") dt = datetime.datetime.now() if args.use_save_interface: state = kb_interface.update() diff --git a/gello/agents/gello_agent.py b/gello/agents/gello_agent.py index fca60c3..f2b0c61 100644 --- a/gello/agents/gello_agent.py +++ b/gello/agents/gello_agent.py @@ -29,9 +29,9 @@ class DynamixelRobotConfig: assert len(self.joint_ids) == len(self.joint_offsets) assert len(self.joint_ids) == len(self.joint_signs) - def make_robot( - self, port: str = "/dev/ttyUSB0", start_joints: Optional[np.ndarray] = None - ) -> DynamixelRobot: + def make_robot(self, + port: str = "/dev/ttyUSB0", + start_joints: Optional[np.ndarray] = None) -> DynamixelRobot: return DynamixelRobot( joint_ids=self.joint_ids, joint_offsets=list(self.joint_offsets), @@ -45,68 +45,73 @@ class DynamixelRobotConfig: PORT_CONFIG_MAP: Dict[str, DynamixelRobotConfig] = { # xArm - # "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT3M9NVB-if00-port0": DynamixelRobotConfig( - # joint_ids=(1, 2, 3, 4, 5, 6, 7), - # joint_offsets=( - # 2 * np.pi / 2, - # 2 * np.pi / 2, - # 2 * np.pi / 2, - # 2 * np.pi / 2, - # -1 * np.pi / 2 + 2 * np.pi, - # 1 * np.pi / 2, - # 1 * np.pi / 2, - # ), - # joint_signs=(1, 1, 1, 1, 1, 1, 1), - # gripper_config=(8, 279, 279 - 50), - # ), + "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT94VP8U-if00-port0": + DynamixelRobotConfig( + joint_ids=(1, 2, 3, 4, 5, 6, 7), + joint_offsets=( + 1 * np.pi / 2, + 1 * np.pi / 2, + 1 * np.pi / 2, + 1 * np.pi / 2, + 1 * np.pi / 2, + 1 * np.pi / 2, + 1 * np.pi / 2, + ), + joint_signs=(1, 1, 1, 1, 1, 1, 1), + gripper_config=(8, 115.024609375, 73.224609375), + ), # panda # "/dev/cu.usbserial-FT3M9NVB": DynamixelRobotConfig( - "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT3M9NVB-if00-port0": DynamixelRobotConfig( - joint_ids=(1, 2, 3, 4, 5, 6, 7), - joint_offsets=( - 3 * np.pi / 2, - 2 * np.pi / 2, - 1 * np.pi / 2, - 4 * np.pi / 2, - -2 * np.pi / 2 + 2 * np.pi, - 3 * np.pi / 2, - 4 * np.pi / 2, + "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT3M9NVB-if00-port0": + DynamixelRobotConfig( + joint_ids=(1, 2, 3, 4, 5, 6, 7), + joint_offsets=( + 3 * np.pi / 2, + 2 * np.pi / 2, + 1 * np.pi / 2, + 4 * np.pi / 2, + -2 * np.pi / 2 + 2 * np.pi, + 3 * np.pi / 2, + 4 * np.pi / 2, + ), + joint_signs=(1, -1, 1, 1, 1, -1, 1), + gripper_config=(8, 195, 152), ), - joint_signs=(1, -1, 1, 1, 1, -1, 1), - gripper_config=(8, 195, 152), - ), # Left UR - "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT7WBEIA-if00-port0": DynamixelRobotConfig( - joint_ids=(1, 2, 3, 4, 5, 6), - joint_offsets=( - 0, - 1 * np.pi / 2 + np.pi, - np.pi / 2 + 0 * np.pi, - 0 * np.pi + np.pi / 2, - np.pi - 2 * np.pi / 2, - -1 * np.pi / 2 + 2 * np.pi, + "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT7WBEIA-if00-port0": + DynamixelRobotConfig( + joint_ids=(1, 2, 3, 4, 5, 6), + joint_offsets=( + 0, + 1 * np.pi / 2 + np.pi, + np.pi / 2 + 0 * np.pi, + 0 * np.pi + np.pi / 2, + np.pi - 2 * np.pi / 2, + -1 * np.pi / 2 + 2 * np.pi, + ), + joint_signs=(1, 1, -1, 1, 1, 1), + gripper_config=(7, 20, -22), ), - joint_signs=(1, 1, -1, 1, 1, 1), - gripper_config=(7, 20, -22), - ), # Right UR - "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT7WBG6A-if00-port0": DynamixelRobotConfig( - joint_ids=(1, 2, 3, 4, 5, 6), - joint_offsets=( - np.pi + 0 * np.pi, - 2 * np.pi + np.pi / 2, - 2 * np.pi + np.pi / 2, - 2 * np.pi + np.pi / 2, - 1 * np.pi, - 3 * np.pi / 2, + "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT7WBG6A-if00-port0": + DynamixelRobotConfig( + joint_ids=(1, 2, 3, 4, 5, 6), + joint_offsets=( + np.pi + 0 * np.pi, + 2 * np.pi + np.pi / 2, + 2 * np.pi + np.pi / 2, + 2 * np.pi + np.pi / 2, + 1 * np.pi, + 3 * np.pi / 2, + ), + joint_signs=(1, 1, -1, 1, 1, 1), + gripper_config=(7, 286, 248), ), - joint_signs=(1, 1, -1, 1, 1, 1), - gripper_config=(7, 286, 248), - ), } class GelloAgent(Agent): + def __init__( self, port: str, @@ -114,18 +119,18 @@ class GelloAgent(Agent): start_joints: Optional[np.ndarray] = None, ): if dynamixel_config is not None: - self._robot = dynamixel_config.make_robot( - port=port, start_joints=start_joints - ) + self._robot = dynamixel_config.make_robot(port=port, + start_joints=start_joints) else: assert os.path.exists(port), port assert port in PORT_CONFIG_MAP, f"Port {port} not in config map" config = PORT_CONFIG_MAP[port] - self._robot = config.make_robot(port=port, start_joints=start_joints) + self._robot = config.make_robot(port=port, + start_joints=start_joints) def act(self, obs: Dict[str, np.ndarray]) -> np.ndarray: - return self._robot.get_joint_state() + # return self._robot.get_joint_state() dyna_joints = self._robot.get_joint_state() # current_q = dyna_joints[:-1] # last one dim is the gripper current_gripper = dyna_joints[-1] # last one dim is the gripper @@ -137,3 +142,8 @@ class GelloAgent(Agent): else: self._robot.set_torque_mode(False) return dyna_joints + + def get_gello_joint_state(self) -> Tuple[np.ndarray, float]: + dyna_joints = self._robot.get_joint_state() + gripper = dyna_joints[-1] + return dyna_joints[:-1], gripper diff --git a/gello/dynamixel/driver.py b/gello/dynamixel/driver.py index 89325c7..3e0dfd5 100644 --- a/gello/dynamixel/driver.py +++ b/gello/dynamixel/driver.py @@ -145,6 +145,33 @@ class DynamixelDriver(DynamixelDriverProtocol): self._stop_thread = Event() self._start_reading_thread() + def move_single_joint(self, dxl_id, joint_angle: float): + if not self._torque_enabled: + raise RuntimeError("Torque must be enabled to set joint angles") + + result, error = self._packetHandler.write4ByteTxRx( + self._portHandler, dxl_id, ADDR_GOAL_POSITION, int(joint_angle * 2048 / np.pi) + ) + + if result != COMM_SUCCESS: + print(f"move_single_joint: {dxl_id}, {joint_angle}, {result}, {error}") + # raise RuntimeError(f"Failed to write goal position for Dynamixel with ID {dxl_id}") + + + + def move_joints(self, joint_angles: Sequence[float]): + if len(joint_angles) != len(self._ids): + raise ValueError( + "The length of joint_angles must match the number of servos" + ) + if not self._torque_enabled: + raise RuntimeError("Torque must be enabled to set joint angles") + # Write goal position + + for dxl_id, angle in zip(self._ids, joint_angles): + self.move_single_joint(dxl_id, angle) + + def set_joints(self, joint_angles: Sequence[float]): if len(joint_angles) != len(self._ids): raise ValueError( @@ -248,7 +275,7 @@ class DynamixelDriver(DynamixelDriverProtocol): def main(): # Set the port, baudrate, and servo IDs - ids = [1] + ids = [1,2,3,4,5,6,7] # Create a DynamixelDriver instance try: @@ -256,15 +283,34 @@ def main(): except FileNotFoundError: driver = DynamixelDriver(ids, port="/dev/cu.usbserial-FT7WBMUB") + joint_angles = driver.get_joints() + print(f"Joint angles for IDs {ids}: {joint_angles}") + # Test setting torque mode driver.set_torque_mode(True) - driver.set_torque_mode(False) + # driver.set_torque_mode(False) + + # driver.move_single_joint(1, 4) + # driver.move_single_joint(2, 0.2) + # driver.move_single_joint(3, 2) + # driver.move_single_joint(4, 3) + # driver.move_single_joint(5, 1.5) + # driver.move_single_joint(6, 2.5) + # driver.move_single_joint(7, 0.25) + + init_angles = np.array([4,0,2,2.5,1.5,3,3.5]) + driver.move_joints(init_angles) # Test reading the joint angles try: while True: joint_angles = driver.get_joints() print(f"Joint angles for IDs {ids}: {joint_angles}") + next_joint = joint_angles - 0.05 + time.sleep(1.0) + print(f"next_joint: {next_joint}") + driver.move_joints(joint_angles) + time.sleep(5.0) # print(f"Joint angles for IDs {ids[1]}: {joint_angles[1]}") except KeyboardInterrupt: driver.close() diff --git a/gello/robots/dynamixel.py b/gello/robots/dynamixel.py index 96d3897..9a221cb 100644 --- a/gello/robots/dynamixel.py +++ b/gello/robots/dynamixel.py @@ -30,6 +30,7 @@ class DynamixelRobot(Robot): if gripper_config is not None: assert joint_offsets is not None assert joint_signs is not None + print("add gripper...") # joint_ids.append(gripper_config[0]) # joint_offsets.append(0.0) @@ -41,6 +42,7 @@ class DynamixelRobot(Robot): gripper_config[1] * np.pi / 180, gripper_config[2] * np.pi / 180, ) + print(f"joint_ids: {joint_ids}") else: self.gripper_open_close = None @@ -109,10 +111,13 @@ class DynamixelRobot(Robot): if self.gripper_open_close is not None: # map pos to [0, 1] + # print(f"pos: {pos[-1]}") + # print(f"gripper_open_close: {self.gripper_open_close}") g_pos = (pos[-1] - self.gripper_open_close[0]) / ( self.gripper_open_close[1] - self.gripper_open_close[0] ) g_pos = min(max(0, g_pos), 1) + # print(f"g_pos: {g_pos}") pos[-1] = g_pos if self._last_pos is None: @@ -125,6 +130,7 @@ class DynamixelRobot(Robot): return pos def command_joint_state(self, joint_state: np.ndarray) -> None: + print(f"Command driver here as {(joint_state + self._joint_offsets).tolist()}") self._driver.set_joints((joint_state + self._joint_offsets).tolist()) def set_torque_mode(self, mode: bool): diff --git a/installdeps.sh b/installdeps.sh new file mode 100755 index 0000000..6eee164 --- /dev/null +++ b/installdeps.sh @@ -0,0 +1,7 @@ +#!/bin/bash + +git submodule init +git submodule update +pip install -r requirements.txt +pip install -e . +pip install -e third_party/DynamixelSDK/python From 3fe1373edbfcad65778c35e548e051ea1df2ecee Mon Sep 17 00:00:00 2001 From: Yuwei Xia Date: Fri, 27 Sep 2024 11:01:32 -0700 Subject: [PATCH 2/7] Fake controller --- gello/agents/gello_agent.py | 6 ++++ gello/dynamixel/driver.py | 63 +++++++++++++++++-------------------- 2 files changed, 35 insertions(+), 34 deletions(-) diff --git a/gello/agents/gello_agent.py b/gello/agents/gello_agent.py index f2b0c61..65bff3a 100644 --- a/gello/agents/gello_agent.py +++ b/gello/agents/gello_agent.py @@ -147,3 +147,9 @@ class GelloAgent(Agent): dyna_joints = self._robot.get_joint_state() gripper = dyna_joints[-1] return dyna_joints[:-1], gripper + + +class FakeGelloAgent(Agent): + + def __init__(self): + self.gripper_state = 0.0 diff --git a/gello/dynamixel/driver.py b/gello/dynamixel/driver.py index 3e0dfd5..63e95c2 100644 --- a/gello/dynamixel/driver.py +++ b/gello/dynamixel/driver.py @@ -26,6 +26,7 @@ TORQUE_DISABLE = 0 class DynamixelDriverProtocol(Protocol): + def set_joints(self, joint_angles: Sequence[float]): """Set the joint angles for the Dynamixel servos. @@ -63,6 +64,7 @@ class DynamixelDriverProtocol(Protocol): class FakeDynamixelDriver(DynamixelDriverProtocol): + def __init__(self, ids: Sequence[int]): self._ids = ids self._joint_angles = np.zeros(len(ids), dtype=int) @@ -71,8 +73,7 @@ class FakeDynamixelDriver(DynamixelDriverProtocol): def set_joints(self, joint_angles: Sequence[float]): if len(joint_angles) != len(self._ids): raise ValueError( - "The length of joint_angles must match the number of servos" - ) + "The length of joint_angles must match the number of servos") if not self._torque_enabled: raise RuntimeError("Torque must be enabled to set joint angles") self._joint_angles = np.array(joint_angles) @@ -91,9 +92,11 @@ class FakeDynamixelDriver(DynamixelDriverProtocol): class DynamixelDriver(DynamixelDriverProtocol): - def __init__( - self, ids: Sequence[int], port: str = "/dev/ttyUSB0", baudrate: int = 57600 - ): + + def __init__(self, + ids: Sequence[int], + port: str = "/dev/ttyUSB0", + baudrate: int = 57600): """Initialize the DynamixelDriver class. Args: @@ -132,8 +135,7 @@ class DynamixelDriver(DynamixelDriverProtocol): for dxl_id in self._ids: if not self._groupSyncRead.addParam(dxl_id): raise RuntimeError( - f"Failed to add parameter for Dynamixel with ID {dxl_id}" - ) + f"Failed to add parameter for Dynamixel with ID {dxl_id}") # Disable torque for each Dynamixel servo self._torque_enabled = False @@ -148,22 +150,21 @@ class DynamixelDriver(DynamixelDriverProtocol): def move_single_joint(self, dxl_id, joint_angle: float): if not self._torque_enabled: raise RuntimeError("Torque must be enabled to set joint angles") - + result, error = self._packetHandler.write4ByteTxRx( - self._portHandler, dxl_id, ADDR_GOAL_POSITION, int(joint_angle * 2048 / np.pi) - ) - + self._portHandler, dxl_id, ADDR_GOAL_POSITION, + int(joint_angle * 2048 / np.pi)) + if result != COMM_SUCCESS: - print(f"move_single_joint: {dxl_id}, {joint_angle}, {result}, {error}") + print( + f"move_single_joint: {dxl_id}, {joint_angle}, {result}, {error}" + ) # raise RuntimeError(f"Failed to write goal position for Dynamixel with ID {dxl_id}") - - def move_joints(self, joint_angles: Sequence[float]): if len(joint_angles) != len(self._ids): raise ValueError( - "The length of joint_angles must match the number of servos" - ) + "The length of joint_angles must match the number of servos") if not self._torque_enabled: raise RuntimeError("Torque must be enabled to set joint angles") # Write goal position @@ -171,12 +172,10 @@ class DynamixelDriver(DynamixelDriverProtocol): for dxl_id, angle in zip(self._ids, joint_angles): self.move_single_joint(dxl_id, angle) - def set_joints(self, joint_angles: Sequence[float]): if len(joint_angles) != len(self._ids): raise ValueError( - "The length of joint_angles must match the number of servos" - ) + "The length of joint_angles must match the number of servos") if not self._torque_enabled: raise RuntimeError("Torque must be enabled to set joint angles") @@ -194,12 +193,10 @@ class DynamixelDriver(DynamixelDriverProtocol): # Add goal position value to the Syncwrite parameter storage dxl_addparam_result = self._groupSyncWrite.addParam( - dxl_id, param_goal_position - ) + dxl_id, param_goal_position) if not dxl_addparam_result: raise RuntimeError( - f"Failed to set joint angle for Dynamixel with ID {dxl_id}" - ) + f"Failed to set joint angle for Dynamixel with ID {dxl_id}") # Syncwrite goal position dxl_comm_result = self._groupSyncWrite.txPacket() @@ -217,8 +214,7 @@ class DynamixelDriver(DynamixelDriverProtocol): with self._lock: for dxl_id in self._ids: dxl_comm_result, dxl_error = self._packetHandler.write1ByteTxRx( - self._portHandler, dxl_id, ADDR_TORQUE_ENABLE, torque_value - ) + self._portHandler, dxl_id, ADDR_TORQUE_ENABLE, torque_value) if dxl_comm_result != COMM_SUCCESS or dxl_error != 0: print(dxl_comm_result) print(dxl_error) @@ -241,15 +237,14 @@ class DynamixelDriver(DynamixelDriverProtocol): _joint_angles = np.zeros(len(self._ids), dtype=int) dxl_comm_result = self._groupSyncRead.txRxPacket() if dxl_comm_result != COMM_SUCCESS: - print(f"warning, comm failed: {dxl_comm_result}") + # print(f"warning, comm failed: {dxl_comm_result}") continue for i, dxl_id in enumerate(self._ids): - if self._groupSyncRead.isAvailable( - dxl_id, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION - ): + if self._groupSyncRead.isAvailable(dxl_id, + ADDR_PRESENT_POSITION, + LEN_PRESENT_POSITION): angle = self._groupSyncRead.getData( - dxl_id, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION - ) + dxl_id, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION) angle = np.int32(np.uint32(angle)) _joint_angles[i] = angle else: @@ -275,7 +270,7 @@ class DynamixelDriver(DynamixelDriverProtocol): def main(): # Set the port, baudrate, and servo IDs - ids = [1,2,3,4,5,6,7] + ids = [1, 2, 3, 4, 5, 6, 7] # Create a DynamixelDriver instance try: @@ -289,7 +284,7 @@ def main(): # Test setting torque mode driver.set_torque_mode(True) # driver.set_torque_mode(False) - + # driver.move_single_joint(1, 4) # driver.move_single_joint(2, 0.2) # driver.move_single_joint(3, 2) @@ -298,7 +293,7 @@ def main(): # driver.move_single_joint(6, 2.5) # driver.move_single_joint(7, 0.25) - init_angles = np.array([4,0,2,2.5,1.5,3,3.5]) + init_angles = np.array([4, 0, 2, 2.5, 1.5, 3, 3.5]) driver.move_joints(init_angles) # Test reading the joint angles From 7dad73f4b743bff7a4af8d37bb68f7f40babe401 Mon Sep 17 00:00:00 2001 From: emfebert Date: Fri, 27 Sep 2024 15:13:51 -0700 Subject: [PATCH 3/7] Update dynamixel_sdk --- .bkpBUILD_bak.bazel | 17 +++++++++++++++++ gello/dynamixel/driver.py | 13 ++++++++----- 2 files changed, 25 insertions(+), 5 deletions(-) create mode 100644 .bkpBUILD_bak.bazel diff --git a/.bkpBUILD_bak.bazel b/.bkpBUILD_bak.bazel new file mode 100644 index 0000000..7cb028e --- /dev/null +++ b/.bkpBUILD_bak.bazel @@ -0,0 +1,17 @@ + +load("@rules_python//python:defs.bzl", "py_library", "py_binary") +load("@python_deps//:requirements.bzl", "requirement") + +py_library( + name = "gello_software_repo", + srcs = glob(["*.py", "**/*.py"]), + deps = [ + requirement("numpy"), + requirement("torchvision"), + requirement("torch"), + requirement("ultralytics"), + requirement("opencv-python"), + requirement("pyquaternion"), + ], + visibility = ["//visibility:public"], +) diff --git a/gello/dynamixel/driver.py b/gello/dynamixel/driver.py index 63e95c2..0fbfcd7 100644 --- a/gello/dynamixel/driver.py +++ b/gello/dynamixel/driver.py @@ -3,11 +3,14 @@ from threading import Event, Lock, Thread from typing import Protocol, Sequence import numpy as np -from dynamixel_sdk.group_sync_read import GroupSyncRead -from dynamixel_sdk.group_sync_write import GroupSyncWrite -from dynamixel_sdk.packet_handler import PacketHandler -from dynamixel_sdk.port_handler import PortHandler -from dynamixel_sdk.robotis_def import ( +import pdb + +pdb.set_trace() +from third_party.DynamixelSDK.dynamixel_sdk.group_sync_read import GroupSyncRead +from third_party.DynamixelSDK.dynamixel_sdk.group_sync_write import GroupSyncWrite +from third_party.DynamixelSDK.dynamixel_sdk.packet_handler import PacketHandler +from third_party.DynamixelSDK.dynamixel_sdk.port_handler import PortHandler +from third_party.DynamixelSDK.dynamixel_sdk.robotis_def import ( COMM_SUCCESS, DXL_HIBYTE, DXL_HIWORD, From ca12950433b37fbfdd88b6671589310f31f588ab Mon Sep 17 00:00:00 2001 From: YuweiXia Date: Tue, 22 Oct 2024 10:36:24 -0700 Subject: [PATCH 4/7] update fake controller with getjs --- gello/agents/gello_agent.py | 24 +++++++++++++++++++++--- gello/dynamixel/driver.py | 12 +++++------- 2 files changed, 26 insertions(+), 10 deletions(-) diff --git a/gello/agents/gello_agent.py b/gello/agents/gello_agent.py index 65bff3a..4d8e3e3 100644 --- a/gello/agents/gello_agent.py +++ b/gello/agents/gello_agent.py @@ -6,6 +6,8 @@ import numpy as np from gello.agents.agent import Agent from gello.robots.dynamixel import DynamixelRobot +import time # Import the time module + @dataclass @@ -135,7 +137,7 @@ class GelloAgent(Agent): # current_q = dyna_joints[:-1] # last one dim is the gripper current_gripper = dyna_joints[-1] # last one dim is the gripper - print(current_gripper) + # print(current_gripper) if current_gripper < 0.2: self._robot.set_torque_mode(False) return obs["joint_positions"] @@ -150,6 +152,22 @@ class GelloAgent(Agent): class FakeGelloAgent(Agent): - def __init__(self): - self.gripper_state = 0.0 + self.gripper_state = 1.0 + self.start_time = time.time() # Record the start time + self.initial_offset = np.array([-1.147, -1.061, 2.261, 1.391, 1.987, 1.392, -2.685]) + self.joint_lower_limits = np.full(7, -np.pi/2) + self.joint_upper_limits = np.full(7, np.pi/2) + self.initial_joint_positions = np.random.uniform(self.joint_lower_limits, self.joint_upper_limits) + self.initial_joint_positions = self.initial_joint_positions + self.initial_offset + + def act(self, obs: Dict[str, np.ndarray]) -> np.ndarray: + return obs["joint_positions"] + + def get_gello_joint_state(self) -> Tuple[np.ndarray, float]: + # Compute the elapsed time since initialization + elapsed_time = time.time() - self.start_time + # Create joint positions that increment over time + joint_positions = self.initial_joint_positions + elapsed_time * 0.1 # Increment each joint by 0.1 radians per second + joint_positions = (joint_positions + np.pi) % (2 * np.pi) - np.pi + return joint_positions, self.gripper_state \ No newline at end of file diff --git a/gello/dynamixel/driver.py b/gello/dynamixel/driver.py index 0fbfcd7..8375814 100644 --- a/gello/dynamixel/driver.py +++ b/gello/dynamixel/driver.py @@ -3,14 +3,12 @@ from threading import Event, Lock, Thread from typing import Protocol, Sequence import numpy as np -import pdb -pdb.set_trace() -from third_party.DynamixelSDK.dynamixel_sdk.group_sync_read import GroupSyncRead -from third_party.DynamixelSDK.dynamixel_sdk.group_sync_write import GroupSyncWrite -from third_party.DynamixelSDK.dynamixel_sdk.packet_handler import PacketHandler -from third_party.DynamixelSDK.dynamixel_sdk.port_handler import PortHandler -from third_party.DynamixelSDK.dynamixel_sdk.robotis_def import ( +from dynamixel_sdk.group_sync_read import GroupSyncRead +from dynamixel_sdk.group_sync_write import GroupSyncWrite +from dynamixel_sdk.packet_handler import PacketHandler +from dynamixel_sdk.port_handler import PortHandler +from dynamixel_sdk.robotis_def import ( COMM_SUCCESS, DXL_HIBYTE, DXL_HIWORD, From 77b8f160c450f97d0c0c66ca9042a0bc98e70287 Mon Sep 17 00:00:00 2001 From: YuweiXia Date: Tue, 29 Oct 2024 11:02:23 -0700 Subject: [PATCH 5/7] Deprecate torque mode --- gello/agents/gello_agent.py | 4 ++-- gello/dynamixel/driver.py | 10 +++++----- gello/robots/dynamixel.py | 2 +- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/gello/agents/gello_agent.py b/gello/agents/gello_agent.py index 4d8e3e3..717251b 100644 --- a/gello/agents/gello_agent.py +++ b/gello/agents/gello_agent.py @@ -139,10 +139,10 @@ class GelloAgent(Agent): # print(current_gripper) if current_gripper < 0.2: - self._robot.set_torque_mode(False) + # self._robot.set_torque_mode(False) return obs["joint_positions"] else: - self._robot.set_torque_mode(False) + # self._robot.set_torque_mode(False) return dyna_joints def get_gello_joint_state(self) -> Tuple[np.ndarray, float]: diff --git a/gello/dynamixel/driver.py b/gello/dynamixel/driver.py index 8375814..d7d712d 100644 --- a/gello/dynamixel/driver.py +++ b/gello/dynamixel/driver.py @@ -140,10 +140,10 @@ class DynamixelDriver(DynamixelDriverProtocol): # Disable torque for each Dynamixel servo self._torque_enabled = False - try: - self.set_torque_mode(self._torque_enabled) - except Exception as e: - print(f"port: {port}, {e}") + # try: + # self.set_torque_mode(self._torque_enabled) + # except Exception as e: + # print(f"port: {port}, {e}") self._stop_thread = Event() self._start_reading_thread() @@ -283,7 +283,7 @@ def main(): print(f"Joint angles for IDs {ids}: {joint_angles}") # Test setting torque mode - driver.set_torque_mode(True) + # driver.set_torque_mode(True) # driver.set_torque_mode(False) # driver.move_single_joint(1, 4) diff --git a/gello/robots/dynamixel.py b/gello/robots/dynamixel.py index 9a221cb..15deb2c 100644 --- a/gello/robots/dynamixel.py +++ b/gello/robots/dynamixel.py @@ -73,7 +73,7 @@ class DynamixelRobot(Robot): if real: self._driver = DynamixelDriver(joint_ids, port=port, baudrate=baudrate) - self._driver.set_torque_mode(False) + # self._driver.set_torque_mode(False) else: self._driver = FakeDynamixelDriver(joint_ids) self._torque_on = False From a14465a6ac0449bcf7db5df339752ce5e5dc4069 Mon Sep 17 00:00:00 2001 From: YuweiXia Date: Tue, 29 Oct 2024 16:55:03 -0700 Subject: [PATCH 6/7] update js offset --- gello/agents/gello_agent.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gello/agents/gello_agent.py b/gello/agents/gello_agent.py index 717251b..0cc7e42 100644 --- a/gello/agents/gello_agent.py +++ b/gello/agents/gello_agent.py @@ -52,14 +52,14 @@ PORT_CONFIG_MAP: Dict[str, DynamixelRobotConfig] = { joint_ids=(1, 2, 3, 4, 5, 6, 7), joint_offsets=( 1 * np.pi / 2, + 3 * np.pi / 2, + 2 * np.pi / 2, 1 * np.pi / 2, 1 * np.pi / 2, 1 * np.pi / 2, - 1 * np.pi / 2, - 1 * np.pi / 2, - 1 * np.pi / 2, + 2 * np.pi / 2, ), - joint_signs=(1, 1, 1, 1, 1, 1, 1), + joint_signs=(1, -1, 1, 1, 1, 1, 1), gripper_config=(8, 115.024609375, 73.224609375), ), # panda From 69f2453e2dfd64cb15125b1cc4bc3494c63097ae Mon Sep 17 00:00:00 2001 From: YuweiXia Date: Tue, 26 Nov 2024 14:15:48 -0800 Subject: [PATCH 7/7] big gello --- gello/agents/gello_agent.py | 18 ++++++++++-------- scripts/gello_get_offset.py | 4 ++-- 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/gello/agents/gello_agent.py b/gello/agents/gello_agent.py index 0cc7e42..b332266 100644 --- a/gello/agents/gello_agent.py +++ b/gello/agents/gello_agent.py @@ -50,17 +50,19 @@ PORT_CONFIG_MAP: Dict[str, DynamixelRobotConfig] = { "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT94VP8U-if00-port0": DynamixelRobotConfig( joint_ids=(1, 2, 3, 4, 5, 6, 7), + # 7*np.pi/4, 4*np.pi/4, 2*np.pi/4, 0*np.pi/4, 4*np.pi/4, 4*np.pi/4, 10*np.pi/4 + # 3*np.pi/4, 4*np.pi/4, 14*np.pi/4, 0*np.pi/4, -4*np.pi/4, 4*np.pi/4, 2*np.pi/4 joint_offsets=( - 1 * np.pi / 2, - 3 * np.pi / 2, - 2 * np.pi / 2, - 1 * np.pi / 2, - 1 * np.pi / 2, - 1 * np.pi / 2, - 2 * np.pi / 2, + 3 * np.pi / 4, + 4 * np.pi / 4, + 14 * np.pi / 4, + 0 * np.pi / 4, + -4 * np.pi / 4, + 4 * np.pi / 4, + 2 * np.pi / 4, ), joint_signs=(1, -1, 1, 1, 1, 1, 1), - gripper_config=(8, 115.024609375, 73.224609375), + gripper_config=(8, 330.79609375, 288.99609375), ), # panda # "/dev/cu.usbserial-FT3M9NVB": DynamixelRobotConfig( diff --git a/scripts/gello_get_offset.py b/scripts/gello_get_offset.py index 4e5aa8c..aca790a 100644 --- a/scripts/gello_get_offset.py +++ b/scripts/gello_get_offset.py @@ -65,7 +65,7 @@ def get_config(args: Args) -> None: best_offset = 0 best_error = 1e6 for offset in np.linspace( - -8 * np.pi, 8 * np.pi, 8 * 4 + 1 + -8 * np.pi, 8 * np.pi, 8 * 8 + 1 ): # intervals of pi/2 error = get_error(offset, i, curr_joints) if error < best_error: @@ -76,7 +76,7 @@ def get_config(args: Args) -> None: 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]) + + ", ".join([f"{int(np.round(x/(np.pi/4)))}*np.pi/4" for x in best_offsets]) + " ]", ) if args.gripper: