From 65601672ecb29b294036d0770d4314407fd4ea85 Mon Sep 17 00:00:00 2001 From: youliang Date: Wed, 10 Apr 2024 14:04:57 -0700 Subject: [PATCH 1/2] able to check motor status Signed-off-by: youliang --- .../widowx_controller/widowx_controller.py | 3 ++ widowx_envs/widowx_envs/widowx_env_service.py | 30 ++++++++++++++----- 2 files changed, 26 insertions(+), 7 deletions(-) diff --git a/widowx_envs/widowx_controller/src/widowx_controller/widowx_controller.py b/widowx_envs/widowx_controller/src/widowx_controller/widowx_controller.py index 14d1c72..7ba159e 100644 --- a/widowx_envs/widowx_controller/src/widowx_controller/widowx_controller.py +++ b/widowx_envs/widowx_controller/src/widowx_controller/widowx_controller.py @@ -223,6 +223,9 @@ def reboot_motor(self, joint_name: str): except rospy.ServiceException as e: print("Service call failed:", e) + def check_motor_status(self): + return self.bot.dxl.robot_get_motor_registers("group", "all", "Hardware_Error_Status") + def clean_shutdown(self): pid = os.getpid() logging.getLogger('robot_logger').info('Exiting example w/ pid: {}'.format(pid)) diff --git a/widowx_envs/widowx_envs/widowx_env_service.py b/widowx_envs/widowx_envs/widowx_env_service.py index 2c82d58..a89c817 100644 --- a/widowx_envs/widowx_envs/widowx_env_service.py +++ b/widowx_envs/widowx_envs/widowx_env_service.py @@ -39,7 +39,7 @@ class WidowXConfigs: DefaultActionConfig = ActionConfig( port_number=5556, action_keys=["init", "move", "gripper", "reset", "step_action", "reboot_motor"], - observation_keys=["image", "state", "full_image"], + observation_keys=["image", "state", "full_image", "motor_status"], broadcast_port=5556 + 1, ) @@ -97,7 +97,6 @@ def start(self, threaded: bool = False): def stop(self): """Stop the server.""" self.__server.stop() - return WidowXStatus.SUCCESS def init_robot(self, env_params, image_size): """Public method to init the robot""" @@ -172,17 +171,24 @@ def __reboot_motor(self, payload) -> WidowXStatus: def __observe(self, types: list) -> dict: if self.bridge_env: - # we will default return image and proprio only + # we will default return all observations obs = self.bridge_env.current_obs() obs = { - "image": obs["image"], + "image": mat_to_jpeg(obs["image"]), # lossy but faster "state": obs["state"], "full_image": mat_to_jpeg(obs["full_image"][0]) # faster } + if "motor_status" in types: + values = self.bridge_env.controller().check_motor_status().values + # TODO: assert the length of values + int_value = np.array(values, dtype=np.uint8) + obs["motor_status"] = int_value else: # use dummy img with random noise img = np.random.randint(0, 255, (480, 640, 3), dtype=np.uint8) obs = {"image": img, "state": {}, "full_image": img} + if "motor_status" in types: + obs["motor_status"] = np.zeros(10, dtype=np.uint8) print_red("WARNING: No bridge env not initialized.") return obs @@ -301,24 +307,34 @@ def get_observation(self) -> Optional[dict]: Get the current camera image and proprioceptive state. :return a dict of observations """ - res = self.__client.obs() + res = self.__client.obs(["image", "state", "full_image"]) if res is None: return None # NOTE: this is a lossy conversion, but faster data transfer + res["image"] = jpeg_to_mat(res["image"]) res["full_image"] = jpeg_to_mat(res["full_image"]) return res + def get_motor_status(self) -> Optional[dict]: + """ + Get the status of the motors. + :return a dict of motor status + """ + res = self.__client.obs(["motor_status"]) + return None if res is None else res["motor_status"] + def stop(self): """Stop the client.""" self.__client.stop() - def reboot_motor(self, joint_name: str): + def reboot_motor(self, joint_name: str) -> WidowXStatus: """Experimentation: Force Reboot the motor. Supported joint names: - waist, shoulder, elbow, forearm_roll, - wrist_angle, wrist_rotate, gripper, left_finger, right_finger """ - self.__client.act("reboot_motor", {"joint_name": joint_name}) + res = self.__client.act("reboot_motor", {"joint_name": joint_name}) + return WidowXStatus.NO_CONNECTION if res is None else res["status"] ############################################################################## From 4f4cd8463ab4b7f936843940bea2e523753c7eae Mon Sep 17 00:00:00 2001 From: youliang Date: Wed, 10 Apr 2024 17:57:08 -0700 Subject: [PATCH 2/2] update test with compliant wip Signed-off-by: youliang --- README.md | 1 + compliant_wx250s.yaml | 153 ++++++++++++++++++ docker-compose.yml | 2 + widowx_envs/Dockerfile | 6 + widowx_envs/widowx_envs/widowx_env_service.py | 23 +-- 5 files changed, 176 insertions(+), 9 deletions(-) create mode 100644 compliant_wx250s.yaml diff --git a/README.md b/README.md index 75278fa..4597a99 100644 --- a/README.md +++ b/README.md @@ -24,6 +24,7 @@ This builds the `robonet-base` image, which contains all of the ROS dependencies Once this is running, you can execute commands in the running container like so: ```bash +cd bridge_data_robot docker compose exec robonet bash -lic "go_sleep" ``` diff --git a/compliant_wx250s.yaml b/compliant_wx250s.yaml new file mode 100644 index 0000000..426be20 --- /dev/null +++ b/compliant_wx250s.yaml @@ -0,0 +1,153 @@ +port: /dev/ttyDXL + +joint_order: [waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate, gripper] +sleep_positions: [0, -1.80, 1.55, 0, 0.8, 0, 0] + +joint_state_publisher: + update_rate: 100 + publish_states: true + topic_name: joint_states + +groups: + arm: [waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate] + +# experimentation mode (YL) +compliant_control: + waist: + lower_limit: -700.0 + upper_limit: 700.0 + p_gain: 0.00006 + clip_control_limit: 0.1 + shoulder: + lower_limit: -850.0 + upper_limit: 850.0 + p_gain: 0.00006 + clip_control_limit: 0.1 + elbow: + lower_limit: -500.0 + upper_limit: 500.0 + p_gain: 0.0001 + clip_control_limit: 0.1 + forearm_roll: + lower_limit: -300.0 + upper_limit: 300.0 + p_gain: 0.0001 + clip_control_limit: 0.12 + wrist_angle: + lower_limit: -300.0 + upper_limit: 300.0 + p_gain: 0.001 + clip_control_limit: 0.12 + wrist_rotate: + lower_limit: -300.0 + upper_limit: 300.0 + p_gain: 0.001 + clip_control_limit: 0.12 + +grippers: + gripper: + horn_radius: 0.014 + arm_length: 0.024 + left_finger: left_finger + right_finger: right_finger + +shadows: + shoulder: + shadow_list: [shoulder_shadow] + calibrate: true + elbow: + shadow_list: [elbow_shadow] + calibrate: true + +sisters: + +motors: + waist: + ID: 1 + Baud_Rate: 3 + Return_Delay_Time: 0 + Drive_Mode: 0 + Velocity_Limit: 131 + Min_Position_Limit: 0 + Max_Position_Limit: 4095 + Secondary_ID: 255 + + shoulder: + ID: 2 + Baud_Rate: 3 + Return_Delay_Time: 0 + Drive_Mode: 0 + Velocity_Limit: 131 + Min_Position_Limit: 819 + Max_Position_Limit: 3345 + Secondary_ID: 255 + + shoulder_shadow: + ID: 3 + Baud_Rate: 3 + Return_Delay_Time: 0 + Drive_Mode: 1 + Velocity_Limit: 131 + Min_Position_Limit: 819 + Max_Position_Limit: 3345 + Secondary_ID: 2 + + elbow: + ID: 4 + Baud_Rate: 3 + Return_Delay_Time: 0 + Drive_Mode: 0 + Velocity_Limit: 131 + Min_Position_Limit: 648 + Max_Position_Limit: 3094 + Secondary_ID: 255 + + elbow_shadow: + ID: 5 + Baud_Rate: 3 + Return_Delay_Time: 0 + Drive_Mode: 1 + Velocity_Limit: 131 + Min_Position_Limit: 648 + Max_Position_Limit: 3094 + Secondary_ID: 4 + + forearm_roll: + ID: 6 + Baud_Rate: 3 + Return_Delay_Time: 0 + Drive_Mode: 0 + Velocity_Limit: 131 + Min_Position_Limit: 0 + Max_Position_Limit: 4095 + Secondary_ID: 255 + + wrist_angle: + ID: 7 + Baud_Rate: 3 + Return_Delay_Time: 0 + Drive_Mode: 1 + Velocity_Limit: 131 + Min_Position_Limit: 910 + Max_Position_Limit: 3447 + Secondary_ID: 255 + + wrist_rotate: + ID: 8 + Baud_Rate: 3 + Return_Delay_Time: 0 + Drive_Mode: 0 + Velocity_Limit: 131 + Min_Position_Limit: 0 + Max_Position_Limit: 4095 + Secondary_ID: 255 + + gripper: + ID: 9 + Baud_Rate: 3 + Return_Delay_Time: 0 + Drive_Mode: 0 + Velocity_Limit: 131 + Min_Position_Limit: 0 + Max_Position_Limit: 4095 + Secondary_ID: 255 diff --git a/docker-compose.yml b/docker-compose.yml index ff1c69c..2023595 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -24,6 +24,8 @@ services: # $HOME/widowx_data:/home/robonet/trainingdata - ./widowx_envs:/home/robonet/widowx_envs - /dev:/dev # for host tty access + # NOTE (YL): Experimental: Use custom compliant control for wx250s arm, default is normal mode + # - $HOME/bridge_data_robot/compliant_wx250s.yaml:/home/robonet/interbotix_ws/src/interbotix_ros_manipulators/interbotix_ros_xsarms/interbotix_xsarm_control/config/wx250s.yaml bridge_data_v2: image: robonet-bridge-data-v2 diff --git a/widowx_envs/Dockerfile b/widowx_envs/Dockerfile index b071ea4..51d0809 100755 --- a/widowx_envs/Dockerfile +++ b/widowx_envs/Dockerfile @@ -73,6 +73,12 @@ ENV DATA=/home/robonet/trainingdata ENV EXP=/home/robonet/experiments ENV REALSENSE_SERIAL=920312072629 +# switch to custom interbotix_ros_core branch for experimental compliant mode support +RUN cd ~/interbotix_ws/src/interbotix_ros_core && \ + git remote add upstream https://github.com/youliangtan/interbotix_ros_core && \ + git fetch upstream && \ + git checkout upstream/noetic + # build interbox ros packages RUN source /opt/ros/noetic/setup.bash \ && source ~/interbotix_ws/devel/setup.bash \ diff --git a/widowx_envs/widowx_envs/widowx_env_service.py b/widowx_envs/widowx_envs/widowx_env_service.py index a89c817..d318893 100644 --- a/widowx_envs/widowx_envs/widowx_env_service.py +++ b/widowx_envs/widowx_envs/widowx_env_service.py @@ -174,14 +174,14 @@ def __observe(self, types: list) -> dict: # we will default return all observations obs = self.bridge_env.current_obs() obs = { - "image": mat_to_jpeg(obs["image"]), # lossy but faster + "image": obs["image"], "state": obs["state"], "full_image": mat_to_jpeg(obs["full_image"][0]) # faster } if "motor_status" in types: values = self.bridge_env.controller().check_motor_status().values - # TODO: assert the length of values int_value = np.array(values, dtype=np.uint8) + assert len(values) == 7 obs["motor_status"] = int_value else: # use dummy img with random noise @@ -311,14 +311,15 @@ def get_observation(self) -> Optional[dict]: if res is None: return None # NOTE: this is a lossy conversion, but faster data transfer - res["image"] = jpeg_to_mat(res["image"]) + res["image"] = res["image"] res["full_image"] = jpeg_to_mat(res["full_image"]) return res - def get_motor_status(self) -> Optional[dict]: + def get_motor_status(self) -> Optional[np.ndarray]: """ - Get the status of the motors. - :return a dict of motor status + Get the status of the motors. array of 7 + :return a np array of motor status [0 ...] + 0 means good, non-zero means error """ res = self.__client.obs(["motor_status"]) return None if res is None else res["motor_status"] @@ -397,6 +398,10 @@ def main(): time.sleep(1) print("Waiting for robot to be ready...") + print(obs) + print(widowx_client.get_motor_status()) + + # widowx_client.step_action(np.array([0.0, 0.0, 0.0, 0, 0, 0, 1.0])) # Coordinate Convention: # - x: forward # - y: left @@ -404,7 +409,7 @@ def main(): # move left up with slanted gripper res = widowx_client.move(np.array([0.2, 0.1, 0.3, 0, 0.47, 0.3])) - assert args.test or res == WidowXStatus.SUCCESS, "move failed" + # assert args.test or res == WidowXStatus.SUCCESS, "move failed" show_video(widowx_client, duration=1.5) # test reboot motor, the gripper should get loosed for a quick moment @@ -419,7 +424,7 @@ def main(): # close gripper print("Closing gripper...") res = widowx_client.move_gripper(0.0) - assert args.test or res == WidowXStatus.SUCCESS, "gripper failed" + # assert args.test or res == WidowXStatus.SUCCESS, "gripper failed" show_video(widowx_client, duration=2.5) print("Run step_action for 25 steps") @@ -430,7 +435,7 @@ def main(): show_video(widowx_client, duration=0.5) # move right down - res = widowx_client.move(np.array([0.2, -0.1, 0.1, 0, 1.57, 0])) + res = widowx_client.move(np.array([0.2, -0.1, 0.1, 0, 1.57, 0]), blocking=True) assert args.test or res == WidowXStatus.SUCCESS, "move failed" show_video(widowx_client, duration=1.5)