diff --git a/experiments/launch_nodes.py b/experiments/launch_nodes.py index 9251711..8c4eb56 100644 --- a/experiments/launch_nodes.py +++ b/experiments/launch_nodes.py @@ -12,7 +12,7 @@ class Args: robot: str = "xarm" robot_port: int = 6001 hostname: str = "127.0.0.1" - robot_ip: str = "192.168.1.10" + robot_ip: str = "192.168.1.226" def launch_robot_server(args: Args): diff --git a/experiments/run_env.py b/experiments/run_env.py index b88c99b..3f76057 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, 0, 0, 90, 0, 90, 0, 0] + [-90, 0, 270, 90, 0, 90, 0, 0] ) # Change this to your own reset joints else: reset_joints = args.start_joints @@ -154,7 +154,7 @@ def main(args): abs_deltas = np.abs(start_pos - joints) id_max_joint_delta = np.argmax(abs_deltas) - max_joint_delta = 0.8 + max_joint_delta = 0.9 if abs_deltas[id_max_joint_delta] > max_joint_delta: id_mask = abs_deltas > max_joint_delta print() @@ -175,30 +175,30 @@ def main(args): joints ), f"agent output dim = {len(start_pos)}, but env dim = {len(joints)}" - max_delta = 0.05 - for _ in range(25): - obs = env.get_obs() - command_joints = agent.act(obs) - current_joints = obs["joint_positions"] - delta = command_joints - current_joints - max_joint_delta = np.abs(delta).max() - if max_joint_delta > max_delta: - delta = delta / max_joint_delta * max_delta - env.step(current_joints + delta) + # max_delta = 0.05 + # for _ in range(25): + # obs = env.get_obs() + # command_joints = agent.act(obs) + # current_joints = obs["joint_positions"] + # delta = command_joints - current_joints + # max_joint_delta = np.abs(delta).max() + # if max_joint_delta > max_delta: + # delta = delta / max_joint_delta * max_delta + # env.step(current_joints + delta) - obs = env.get_obs() - joints = obs["joint_positions"] - action = agent.act(obs) - if (action - joints > 0.5).any(): - print("Action is too big") - - # print which joints are too big - joint_index = np.where(action - joints > 0.8) - for j in joint_index: - print( - f"Joint [{j}], leader: {action[j]}, follower: {joints[j]}, diff: {action[j] - joints[j]}" - ) - exit() + # obs = env.get_obs() + # joints = obs["joint_positions"] + # action = agent.act(obs) + # if (action - joints > 0.5).any(): + # print("Action is too big") + # + # # print which joints are too big + # joint_index = np.where(action - joints > 0.8) + # for j in joint_index: + # print( + # f"Joint [{j}], leader: {action[j]}, follower: {joints[j]}, diff: {action[j] - joints[j]}" + # ) + # exit() if args.use_save_interface: from gello.data_utils.keyboard_interface import KBReset diff --git a/gello/agents/gello_agent.py b/gello/agents/gello_agent.py index b649834..0ea7283 100644 --- a/gello/agents/gello_agent.py +++ b/gello/agents/gello_agent.py @@ -61,20 +61,22 @@ PORT_CONFIG_MAP: Dict[str, DynamixelRobotConfig] = { # ), # panda # "/dev/cu.usbserial-FT3M9NVB": DynamixelRobotConfig( + # "/dev/cu.usbserial-FT89FJNO": DynamixelRobotConfig( # "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT3M9NVB-if00-port0": DynamixelRobotConfig( - "/dev/cu.usbserial-FT89FJNO": DynamixelRobotConfig( + + "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT89FJNO-if00-port0": DynamixelRobotConfig( joint_ids=(1, 2, 3, 4, 5, 6, 7), joint_offsets=( - 1 * np.pi / 2, 2 * np.pi / 2, - 4 * np.pi / 2, + 2 * np.pi / 2, + 0 * np.pi / 2, 0 * np.pi / 2, 2 * np.pi / 2, - 3 * np.pi / 2, - 3 * np.pi / 2, + 1 * np.pi / 2, + 3 * np.pi / 2 - np.pi/3, ), - joint_signs=(1, -1, 1, 1, 1, -1, 1), - gripper_config=(8, 205, 170), + joint_signs=(1, 1, 1, 1, 1, 1, 1), + gripper_config=(8, 209, 168), ), # Left UR "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT7WBEIA-if00-port0": DynamixelRobotConfig( diff --git a/gello/robots/xarm_robot.py b/gello/robots/xarm_robot.py index 366367b..b9cff24 100644 --- a/gello/robots/xarm_robot.py +++ b/gello/robots/xarm_robot.py @@ -118,7 +118,7 @@ class Rate: class XArmRobot(Robot): GRIPPER_OPEN = 800 - GRIPPER_CLOSE = 0 + GRIPPER_CLOSE = 100 # MAX_DELTA = 0.2 DEFAULT_MAX_DELTA = 0.05 @@ -155,9 +155,11 @@ class XArmRobot(Robot): real: bool = True, control_frequency: float = 50.0, max_delta: float = DEFAULT_MAX_DELTA, + use_robotiq: bool = True, ): print(ip) self.real = real + self.use_robotiq = use_robotiq self.max_delta = max_delta if real: from xarm.wrapper import XArmAPI @@ -166,6 +168,12 @@ class XArmRobot(Robot): else: self.robot = None + if self.use_robotiq: + import pyRobotiqGripper + gripper = pyRobotiqGripper.RobotiqGripper() + self.gripper = gripper + #gripper.activate() + self._control_frequency = control_frequency self._clear_error_states() self._set_gripper_position(self.GRIPPER_OPEN) @@ -183,6 +191,7 @@ class XArmRobot(Robot): self.command_thread = threading.Thread(target=self._robot_thread) self.command_thread.start() + def get_state(self) -> RobotState: with self.last_state_lock: return self.last_state @@ -207,33 +216,48 @@ class XArmRobot(Robot): time.sleep(1) self.robot.set_state(state=0) time.sleep(1) - self.robot.set_gripper_enable(True) - time.sleep(1) - self.robot.set_gripper_mode(0) - time.sleep(1) - self.robot.set_gripper_speed(3000) - time.sleep(1) + if self.use_robotiq: + pass + else: + self.robot.set_gripper_enable(True) + time.sleep(1) + self.robot.set_gripper_mode(0) + time.sleep(1) + self.robot.set_gripper_speed(3000) + time.sleep(1) def _get_gripper_pos(self) -> float: if self.robot is None: return 0.0 - code, gripper_pos = self.robot.get_gripper_position() - while code != 0 or gripper_pos is None: - print(f"Error code {code} in get_gripper_position(). {gripper_pos}") - time.sleep(0.001) + if self.use_robotiq: + return self.gripper.getPosition() / 255 + else: code, gripper_pos = self.robot.get_gripper_position() - if code == 22: - self._clear_error_states() + while code != 0 or gripper_pos is None: + print(f"Error code {code} in get_gripper_position(). {gripper_pos}") + time.sleep(0.001) + code, gripper_pos = self.robot.get_gripper_position() + if code == 22: + self._clear_error_states() - normalized_gripper_pos = (gripper_pos - self.GRIPPER_OPEN) / ( - self.GRIPPER_CLOSE - self.GRIPPER_OPEN - ) - return normalized_gripper_pos + normalized_gripper_pos = (gripper_pos - self.GRIPPER_OPEN) / ( + self.GRIPPER_CLOSE - self.GRIPPER_OPEN + ) + return normalized_gripper_pos def _set_gripper_position(self, pos: int) -> None: if self.robot is None: return - self.robot.set_gripper_position(pos, wait=False) + if self.use_robotiq: + pos = 255 - (pos / 800 * 255) + try: + self.gripper.goTo(pos, wait=False) + except Exception as e: + print(e) + print(pos) + raise e + else: + self.robot.set_gripper_position(pos, wait=False) # while self.robot.get_is_moving(): # time.sleep(0.01) @@ -347,12 +371,26 @@ def main(): time.sleep(1) print(robot.get_state()) + print(robot.get_state()) + print(robot.get_state()) + print(robot.get_state()) + print(robot.get_state()) time.sleep(1) print(robot.get_state()) print("end") + robot.command_joint_state(np.zeros(7)) robot.stop() if __name__ == "__main__": + ip = "192.168.1.226" + # from xarm.wrapper import XArmAPI + # robot = XArmAPI(ip, is_radian=True) + # robot.set_mode(1) + # robot.set_mode(1) + # robot.set_mode(1) + # print(robot.get_servo_angle()) + # robot.set_servo_angle_j(np.ones(7)*0.1 + robot.get_servo_angle()[1]) + # print(robot.get_state()) main()