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 @@ PORT_CONFIG_MAP: Dict[str, DynamixelRobotConfig] = { 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 @@ class ArmObservables(composer.Observables): 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 @@ class DynamixelDriver(DynamixelDriverProtocol): # 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 @@ class DynamixelRobot(Robot): 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: