diff --git a/.bkpBUILD_bak.bazel b/.bkpBUILD_bak.bazel new file mode 100644 index 00000000..7cb028ed --- /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/BUILD.bazel b/BUILD.bazel new file mode 100644 index 00000000..b37d1b91 --- /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 bd2319a6..20aaf978 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 fca60c39..b3322669 100644 --- a/gello/agents/gello_agent.py +++ b/gello/agents/gello_agent.py @@ -6,6 +6,8 @@ from gello.agents.agent import Agent from gello.robots.dynamixel import DynamixelRobot +import time # Import the time module + @dataclass @@ -29,9 +31,9 @@ def __post_init__(self): 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 +47,75 @@ def make_robot( 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), + # 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=( + 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, 330.79609375, 288.99609375), + ), # 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,26 +123,53 @@ def __init__( 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 - print(current_gripper) + # 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]: + 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 = 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 89325c75..d7d712db 100644 --- a/gello/dynamixel/driver.py +++ b/gello/dynamixel/driver.py @@ -3,6 +3,7 @@ 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 @@ -26,6 +27,7 @@ class DynamixelDriverProtocol(Protocol): + def set_joints(self, joint_angles: Sequence[float]): """Set the joint angles for the Dynamixel servos. @@ -63,6 +65,7 @@ def close(self): class FakeDynamixelDriver(DynamixelDriverProtocol): + def __init__(self, ids: Sequence[int]): self._ids = ids self._joint_angles = np.zeros(len(ids), dtype=int) @@ -71,8 +74,7 @@ def __init__(self, ids: Sequence[int]): 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 +93,11 @@ def close(self): 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,24 +136,47 @@ def __init__( 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 - 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() + 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( - "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") @@ -167,12 +194,10 @@ def set_joints(self, joint_angles: Sequence[float]): # 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() @@ -190,8 +215,7 @@ def set_torque_mode(self, enable: bool): 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) @@ -214,15 +238,14 @@ def _read_joint_angles(self): _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: @@ -248,7 +271,7 @@ def close(self): 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 +279,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(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) + # 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 96d38976..15deb2c2 100644 --- a/gello/robots/dynamixel.py +++ b/gello/robots/dynamixel.py @@ -30,6 +30,7 @@ def __init__( 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 @@ def __init__( gripper_config[1] * np.pi / 180, gripper_config[2] * np.pi / 180, ) + print(f"joint_ids: {joint_ids}") else: self.gripper_open_close = None @@ -71,7 +73,7 @@ def __init__( 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 @@ -109,10 +111,13 @@ def get_joint_state(self) -> np.ndarray: 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 @@ def get_joint_state(self) -> np.ndarray: 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 00000000..6eee1642 --- /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 diff --git a/scripts/gello_get_offset.py b/scripts/gello_get_offset.py index 4e5aa8ca..aca790a5 100644 --- a/scripts/gello_get_offset.py +++ b/scripts/gello_get_offset.py @@ -65,7 +65,7 @@ def get_error(offset: float, index: int, joint_state: np.ndarray) -> float: 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_error(offset: float, index: int, joint_state: np.ndarray) -> float: 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: