Check motor status and experimental compliant mode

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"
 ```
 

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

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

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 \

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))

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"],
                 "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
+                int_value = np.array(values, dtype=np.uint8)
+                assert len(values) == 7
+                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,35 @@ 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"] = res["image"]
         res["full_image"] = jpeg_to_mat(res["full_image"])
         return res
 
+    def get_motor_status(self) -> Optional[np.ndarray]:
+        """
+        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"]
+
     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"]
 
 ##############################################################################
 
@@ -381,14 +398,18 @@ 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
         #  - z: up
 
         # 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
@@ -403,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")
@@ -414,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)