From 7001842e7af61a89e3551fe5f5e9afe8b3f82cd5 Mon Sep 17 00:00:00 2001 From: Kevin Zakka Date: Thu, 4 Apr 2024 16:27:32 -0700 Subject: [PATCH 1/4] Bugfixes. --- experiments/run_env.py | 3 ++- gello/agents/gello_agent.py | 2 +- gello/dm_control_tasks/arms/manipulator.py | 2 +- gello/dynamixel/driver.py | 4 ++-- gello/robots/dynamixel.py | 6 +++--- requirements.txt | 2 +- scripts/arm_blocks_play.py | 16 ++++++++-------- 7 files changed, 18 insertions(+), 17 deletions(-) diff --git a/experiments/run_env.py b/experiments/run_env.py index bd2319a..57fb4b0 100644 --- a/experiments/run_env.py +++ b/experiments/run_env.py @@ -106,7 +106,8 @@ def main(args): if args.agent == "gello": gello_port = args.gello_port if gello_port is None: - usb_ports = glob.glob("/dev/serial/by-id/*") + # usb_ports = glob.glob("/dev/serial/by-id/*") + usb_ports = glob.glob("/dev/cu.usbserial*") print(f"Found {len(usb_ports)} ports") if len(usb_ports) > 0: gello_port = usb_ports[0] diff --git a/gello/agents/gello_agent.py b/gello/agents/gello_agent.py index fca60c3..6f23564 100644 --- a/gello/agents/gello_agent.py +++ b/gello/agents/gello_agent.py @@ -90,7 +90,7 @@ def make_robot( gripper_config=(7, 20, -22), ), # Right UR - "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT7WBG6A-if00-port0": DynamixelRobotConfig( + "/dev/cu.usbserial-FT7WBG6A": DynamixelRobotConfig( joint_ids=(1, 2, 3, 4, 5, 6), joint_offsets=( np.pi + 0 * np.pi, diff --git a/gello/dm_control_tasks/arms/manipulator.py b/gello/dm_control_tasks/arms/manipulator.py index 6f251e8..068d786 100644 --- a/gello/dm_control_tasks/arms/manipulator.py +++ b/gello/dm_control_tasks/arms/manipulator.py @@ -225,5 +225,5 @@ def proprioception(self): self.flange_position, # self.flange_orientation, # self.flange_velocity, - self.flange_angular_velocity, + # self.flange_angular_velocity, ] + self._collect_from_attachments("proprioception") diff --git a/gello/dynamixel/driver.py b/gello/dynamixel/driver.py index 360c64b..df6a5d7 100644 --- a/gello/dynamixel/driver.py +++ b/gello/dynamixel/driver.py @@ -237,8 +237,8 @@ def get_joints(self) -> np.ndarray: # Return a copy of the joint_angles array to avoid race conditions while self._joint_angles is None: time.sleep(0.1) - with self._lock: - _j = self._joint_angles.copy() + # with self._lock: + _j = self._joint_angles.copy() return _j / 2048.0 * np.pi def close(self): diff --git a/gello/robots/dynamixel.py b/gello/robots/dynamixel.py index d8676f6..67e9d4b 100644 --- a/gello/robots/dynamixel.py +++ b/gello/robots/dynamixel.py @@ -86,11 +86,11 @@ def __init__( if gripper_config is not None: current_joints = current_joints[:-1] start_joints = start_joints[:-1] - for c_joint, s_joint, joint_offset in zip( + for idx, (c_joint, s_joint, joint_offset) in enumerate(zip( current_joints, start_joints, self._joint_offsets - ): + )): new_joint_offsets.append( - np.pi * 2 * np.round((s_joint - c_joint) / (2 * np.pi)) + np.pi * 2 * np.round((-s_joint + c_joint) / (2 * np.pi)) * self._joint_signs[idx] + joint_offset ) if gripper_config is not None: diff --git a/requirements.txt b/requirements.txt index 5ac85ad..b7635ae 100644 --- a/requirements.txt +++ b/requirements.txt @@ -5,7 +5,7 @@ pygame pyspacemouse PyQt6 pyquaternion -pyrealsense2 +# pyrealsense2 pure-python-adb quaternion tyro diff --git a/scripts/arm_blocks_play.py b/scripts/arm_blocks_play.py index 851e85c..8a11e2c 100644 --- a/scripts/arm_blocks_play.py +++ b/scripts/arm_blocks_play.py @@ -18,15 +18,15 @@ class Args: config = DynamixelRobotConfig( joint_ids=(1, 2, 3, 4, 5, 6), joint_offsets=( - -np.pi / 2, - 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, + 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, 20, -22), + gripper_config=(7, 286, 248), ) @@ -40,7 +40,7 @@ def main(args: Args) -> None: action_space = env.action_spec() if args.use_gello: gello = config.make_robot( - port="/dev/cu.usbserial-FT7WBEIA", start_joints=reset_joints_left + port="/dev/cu.usbserial-FT7WBG6A", start_joints=reset_joints_left ) def policy(timestep) -> np.ndarray: From 23892c535d3103ca57c6eb2b5b68277bf7078ebc Mon Sep 17 00:00:00 2001 From: Philipp Wu Date: Thu, 4 Apr 2024 16:31:30 -0700 Subject: [PATCH 2/4] minor --- .pre-commit-config.yaml | 2 +- experiments/run_env.py | 3 +-- gello/agents/gello_agent.py | 2 +- requirements.txt | 2 +- scripts/arm_blocks_play.py | 16 ++++++++-------- 5 files changed, 12 insertions(+), 13 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 7c92e72..5a69727 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -15,7 +15,7 @@ repos: # code format according to black - repo: https://github.com/ambv/black - rev: 23.3.0 + rev: 24.2.0 hooks: - id: black diff --git a/experiments/run_env.py b/experiments/run_env.py index 57fb4b0..bd2319a 100644 --- a/experiments/run_env.py +++ b/experiments/run_env.py @@ -106,8 +106,7 @@ def main(args): if args.agent == "gello": gello_port = args.gello_port if gello_port is None: - # usb_ports = glob.glob("/dev/serial/by-id/*") - usb_ports = glob.glob("/dev/cu.usbserial*") + usb_ports = glob.glob("/dev/serial/by-id/*") print(f"Found {len(usb_ports)} ports") if len(usb_ports) > 0: gello_port = usb_ports[0] diff --git a/gello/agents/gello_agent.py b/gello/agents/gello_agent.py index 6f23564..fca60c3 100644 --- a/gello/agents/gello_agent.py +++ b/gello/agents/gello_agent.py @@ -90,7 +90,7 @@ def make_robot( gripper_config=(7, 20, -22), ), # Right UR - "/dev/cu.usbserial-FT7WBG6A": DynamixelRobotConfig( + "/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, diff --git a/requirements.txt b/requirements.txt index b7635ae..5ac85ad 100644 --- a/requirements.txt +++ b/requirements.txt @@ -5,7 +5,7 @@ pygame pyspacemouse PyQt6 pyquaternion -# pyrealsense2 +pyrealsense2 pure-python-adb quaternion tyro diff --git a/scripts/arm_blocks_play.py b/scripts/arm_blocks_play.py index 8a11e2c..851e85c 100644 --- a/scripts/arm_blocks_play.py +++ b/scripts/arm_blocks_play.py @@ -18,15 +18,15 @@ class Args: config = 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, + -np.pi / 2, + 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, 286, 248), + gripper_config=(7, 20, -22), ) @@ -40,7 +40,7 @@ def main(args: Args) -> None: action_space = env.action_spec() if args.use_gello: gello = config.make_robot( - port="/dev/cu.usbserial-FT7WBG6A", start_joints=reset_joints_left + port="/dev/cu.usbserial-FT7WBEIA", start_joints=reset_joints_left ) def policy(timestep) -> np.ndarray: From abd53a4c2bff723da30420717ebc6f2412d4db13 Mon Sep 17 00:00:00 2001 From: Philipp Wu Date: Thu, 4 Apr 2024 16:36:39 -0700 Subject: [PATCH 3/4] minoir --- README.md | 2 +- gello/dynamixel/driver.py | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 5879adf..0a4081b 100644 --- a/README.md +++ b/README.md @@ -31,7 +31,7 @@ docker build . -t gello:latest We have provided an entry point into the docker container ``` -python experiments/launch.py +python scripts/launch.py ``` # GELLO configuration setup (PLEASE READ) diff --git a/gello/dynamixel/driver.py b/gello/dynamixel/driver.py index df6a5d7..89325c7 100644 --- a/gello/dynamixel/driver.py +++ b/gello/dynamixel/driver.py @@ -20,7 +20,6 @@ ADDR_GOAL_POSITION = 116 LEN_GOAL_POSITION = 4 ADDR_PRESENT_POSITION = 132 -ADDR_PRESENT_POSITION = 140 LEN_PRESENT_POSITION = 4 TORQUE_ENABLE = 1 TORQUE_DISABLE = 0 @@ -257,10 +256,11 @@ def main(): except FileNotFoundError: driver = DynamixelDriver(ids, port="/dev/cu.usbserial-FT7WBMUB") + # Test setting torque mode driver.set_torque_mode(True) driver.set_torque_mode(False) - # Print the joint angles + # Test reading the joint angles try: while True: joint_angles = driver.get_joints() @@ -271,4 +271,4 @@ def main(): if __name__ == "__main__": - main() + main() # Test the driver From 16e09073bf4f24aed1a7d755904c33df4fe92427 Mon Sep 17 00:00:00 2001 From: Philipp Wu Date: Thu, 4 Apr 2024 16:41:57 -0700 Subject: [PATCH 4/4] minor --- .pre-commit-config.yaml | 10 +++++----- gello/dm_control_tasks/arms/manipulator.py | 1 + gello/dm_control_tasks/manipulation/arenas/floors.py | 1 + .../dm_control_tasks/manipulation/tasks/block_play.py | 1 + gello/robots/dynamixel.py | 11 +++++++---- 5 files changed, 15 insertions(+), 9 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 5a69727..1e6dcc3 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -2,26 +2,26 @@ repos: # remove unused python imports - repo: https://github.com/myint/autoflake.git - rev: v2.1.1 + rev: v2.3.1 hooks: - id: autoflake args: ["--in-place", "--remove-all-unused-imports", "--ignore-init-module-imports"] # sort imports - repo: https://github.com/timothycrosley/isort - rev: 5.12.0 + rev: 5.13.2 hooks: - id: isort # code format according to black - repo: https://github.com/ambv/black - rev: 24.2.0 + rev: 24.3.0 hooks: - id: black # check for python styling with flake8 - repo: https://github.com/PyCQA/flake8 - rev: 6.0.0 + rev: 7.0.0 hooks: - id: flake8 additional_dependencies: [ @@ -33,6 +33,6 @@ repos: # cleanup notebooks - repo: https://github.com/kynan/nbstripout - rev: 0.6.1 + rev: 0.7.1 hooks: - id: nbstripout diff --git a/gello/dm_control_tasks/arms/manipulator.py b/gello/dm_control_tasks/arms/manipulator.py index 068d786..d251e48 100644 --- a/gello/dm_control_tasks/arms/manipulator.py +++ b/gello/dm_control_tasks/arms/manipulator.py @@ -1,4 +1,5 @@ """Manipulator composer class.""" + import abc from pathlib import Path from typing import Dict, List, Optional, Tuple, Union diff --git a/gello/dm_control_tasks/manipulation/arenas/floors.py b/gello/dm_control_tasks/manipulation/arenas/floors.py index 0a67d60..7814c05 100644 --- a/gello/dm_control_tasks/manipulation/arenas/floors.py +++ b/gello/dm_control_tasks/manipulation/arenas/floors.py @@ -1,4 +1,5 @@ """Simple floor arenas.""" + from typing import Tuple import numpy as np diff --git a/gello/dm_control_tasks/manipulation/tasks/block_play.py b/gello/dm_control_tasks/manipulation/tasks/block_play.py index 09589a0..5d4135e 100644 --- a/gello/dm_control_tasks/manipulation/tasks/block_play.py +++ b/gello/dm_control_tasks/manipulation/tasks/block_play.py @@ -1,4 +1,5 @@ """A task where a walker must learn to stand.""" + from typing import Optional import numpy as np diff --git a/gello/robots/dynamixel.py b/gello/robots/dynamixel.py index 67e9d4b..96d3897 100644 --- a/gello/robots/dynamixel.py +++ b/gello/robots/dynamixel.py @@ -86,11 +86,14 @@ def __init__( if gripper_config is not None: current_joints = current_joints[:-1] start_joints = start_joints[:-1] - for idx, (c_joint, s_joint, joint_offset) in enumerate(zip( - current_joints, start_joints, self._joint_offsets - )): + for idx, (c_joint, s_joint, joint_offset) in enumerate( + zip(current_joints, start_joints, self._joint_offsets) + ): new_joint_offsets.append( - np.pi * 2 * np.round((-s_joint + c_joint) / (2 * np.pi)) * self._joint_signs[idx] + np.pi + * 2 + * np.round((-s_joint + c_joint) / (2 * np.pi)) + * self._joint_signs[idx] + joint_offset ) if gripper_config is not None: