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..5372804 100644 --- a/gello/agents/gello_agent.py +++ b/gello/agents/gello_agent.py @@ -29,9 +29,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 +45,73 @@ 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), + joint_offsets=( + 3 * np.pi / 2, + 1 * np.pi / 2, + 3 * np.pi / 2, + 1 * np.pi / 2, + 3 * 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 @@ 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 @@ -137,3 +142,8 @@ def act(self, obs: Dict[str, np.ndarray]) -> np.ndarray: else: self._robot.set_torque_mode(False) return dyna_joints + + def getGelloJointState(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 @@ def __init__( 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 @@ 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 +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 @@ 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 @@ -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 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