From 51d800ce154d82c9b4ff68a060bc4b913c0dc863 Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Sun, 23 Mar 2025 15:02:20 +0800 Subject: [PATCH 01/27] 1. auto generating bson_dict 2. configure cameras in yaml 3. fix start joint/action mismatch 4. support newest types 5. pass single arm data collection --- .../tok/airbot_tok_ptk_demonstration.yaml | 52 +++--- .../config_tools/basic_configer.py | 4 +- control_robot_bson.py | 149 +----------------- envs/airbot_com_mmk_env.py | 5 +- robots/airbots/airbot_mmk/airbot_com_mmk2.py | 62 ++++---- .../airbot_mmk/airbot_com_mmk2_bson.py | 58 ++++++- 6 files changed, 117 insertions(+), 213 deletions(-) diff --git a/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk_demonstration.yaml b/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk_demonstration.yaml index b5742d9..2d6e670 100644 --- a/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk_demonstration.yaml +++ b/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk_demonstration.yaml @@ -3,29 +3,41 @@ _target_: robots.airbots.airbot_mmk.airbot_com_mmk2_bson.AIRBOTMMK2 ip: "localhost" # NOTE: If you want to change the order of each part, be careful to change the order of each observation and action in the data conversion script simultaneously -components: ["left_arm", "left_arm_eef", "right_arm", "right_arm_eef"] +components: ["left_arm", "left_arm_eef"] +# components: ["left_arm", "left_arm_eef", "right_arm", "right_arm_eef"] +# USB cameras (only color image types) cameras: - head_camera: ["color"] - # left_camera: ["color"] - # right_camera: ["color"] + head_camera: + image_types: ["color"] + camera_type: "USB" + video_device: "/dev/video0" + # left_camera: + # image_types: ["color"] + # camera_type: "USB" + # video_device: "/dev/video2" + # right_camera: + # image_types: ["color"] + # camera_type: "USB" + # video_device: "/dev/video4" + +# RealSense +# cameras: +# head_camera: +# image_types: ["color"] +# camera_type: "REALSENSE" +# serial_no: "'123456'" +# left_camera: +# image_types: ["color"] +# camera_type: "REALSENSE" +# serial_no: "'123456'" +# right_camera: +# image_types: ["color"] +# camera_type: "REALSENSE" +# serial_no: "'123456'" demonstrate: true default_action: - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - ] + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + # [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] diff --git a/configurations/task_configs/config_tools/basic_configer.py b/configurations/task_configs/config_tools/basic_configer.py index a3ac548..d0bf6dd 100644 --- a/configurations/task_configs/config_tools/basic_configer.py +++ b/configurations/task_configs/config_tools/basic_configer.py @@ -145,8 +145,8 @@ def get_all_config(args: dict, stage: str): # set start joint if all_config.get("start_joint", None) is None: init_states = get_init_states(all_config["load_data"], 0) - all_config["start_action"] = init_states[0] - all_config["start_joint"] = init_states[1] + all_config["start_joint"] = init_states[0] + all_config["start_action"] = init_states[1] # set augmentors all_config["load_data"]["augmentors"]["image"] = task_funcs["image_augmentor"] all_config["augmentors_flag"] = { diff --git a/control_robot_bson.py b/control_robot_bson.py index 1effe7e..5b038c1 100644 --- a/control_robot_bson.py +++ b/control_robot_bson.py @@ -570,154 +570,7 @@ def stop_recording(self): timestamp = 0 start_episode_t = time.perf_counter() # Record one episode - bson_dict: Dict[str, Dict[str, list]] = { - "id": "734ad1c8-66ee-4479-b3cb-41d16c9b2e22", - "timestamp": 1734076528859, - "metadata": { - "driver_version": "1.0.0", - "operator": "manual", - "station_id": "3784D4BA-87AF-47E7-B86D-42CA1904AA77", - "task": "example", - "topics": { - "/action/head/joint_state": { - "description": "", - "type": "jointstate", - "sn": "", - "firmware_version": "0.0.0", - }, - "/action/spine/joint_state": { - "description": "", - "type": "jointstate", - "sn": "", - "firmware_version": "0.0.0", - }, - "/action/left_arm/joint_state": { - "description": "replay", - "type": "jointstate", - "sn": "", - "firmware_version": "0.0.0", - }, - "/action/left_arm_eef/joint_state": { - "description": "replay", - "type": "jointstate", - "sn": "", - "firmware_version": "0.0.0", - }, - "/action/right_arm/joint_state": { - "description": "replay", - "type": "jointstate", - "sn": "", - "firmware_version": "0.0.0", - }, - "/action/right_arm_eef/joint_state": { - "description": "replay", - "type": "jointstate", - "sn": "", - "firmware_version": "0.0.0", - }, - # "/action/eef/pose": { - # "description": "", - # "type": "jointstate", - # "sn": "", - # "firmware_version": "0.0.0", - # }, - # "/action/base/joint_state": { - # "description": "slamtec-athena", - # "type": "jointstate", - # "sn": "", - # "firmware_version": "0.0.0", - # }, - "/observation/head/joint_state": { - "description": "", - "type": "jointstate", - "sn": "", - "firmware_version": "0.0.0", - }, - "/observation/spine/joint_state": { - "description": "", - "type": "jointstate", - "sn": "", - "firmware_version": "0.0.0", - }, - "/observation/left_arm/joint_state": { - "description": "airbot-play-short", - "type": "jointstate", - "sn": "", - "firmware_version": "0.0.0", - }, - "/observation/right_arm/joint_state": { - "description": "airbot-play-short", - "type": "jointstate", - "sn": "", - "firmware_version": "0.0.0", - }, - "/observation/left_arm_eef/joint_state": { - "description": "airbot-play-short", - "type": "jointstate", - "sn": "", - "firmware_version": "0.0.0", - }, - "/observation/right_arm_eef/joint_state": { - "description": "airbot-play-short", - "type": "jointstate", - "sn": "", - "firmware_version": "0.0.0", - }, - "/observation/left_arm/pose": { - "description": "", - "type": "pose", - }, - "/observation/right_arm/pose": { - "description": "", - "type": "pose", - }, - # "/observation/base/joint_state": { - # "description": "slamtec-athena", - # "type": "jointstate", - # "sn": "", - # "firmware_version": "0.0.0", - # }, - "/images/head_camera": { - "description": "DSJ-2062-309", - "type": "image", - "width": 640, - "height": 480, - "encoding": "H264", - "distortion_model": None, - "distortion_params": None, - "intrinsics": None, - "fov": 120.0, - "start_time": 1733377253041, - }, - # "/images/left_camera": { - # "description": "DSJ-2062-309", - # "type": "image", - # "width": 640, - # "height": 480, - # "encoding": "H264", - # "distortion_model": None, - # "distortion_params": None, - # "intrinsics": None, - # "fov": 120.0, - # "start_time": 1733377253041, - # }, - # "/images/right_camera": { - # "description": "DSJ-2062-309", - # "type": "image", - # "width": 640, - # "height": 480, - # "encoding": "H264", - # "distortion_model": None, - # "distortion_params": None, - # "intrinsics": None, - # "fov": 120.0, - # "start_time": 1733377253041, - # }, - }, - "version": "1.2.1", - }, - "data": {}, - } + bson_dict: Dict[str, Dict[str, list]] = robot.bson_dict while timestamp < episode_time_s: start_loop_t = time.perf_counter() diff --git a/envs/airbot_com_mmk_env.py b/envs/airbot_com_mmk_env.py index 95250e5..1bc7460 100644 --- a/envs/airbot_com_mmk_env.py +++ b/envs/airbot_com_mmk_env.py @@ -1,7 +1,6 @@ from robots.airbots.airbot_mmk.airbot_com_mmk2 import AIRBOTMMK2 from robots.common import make_robot_from_yaml import time -import collections import dm_env import numpy as np @@ -23,14 +22,14 @@ def reset(self, sleep_time=0): return self._get_obs() def _get_obs(self): - obs = collections.OrderedDict() + obs = {} obs["qpos"] = [] obs["images"] = {} raw_obs = self.robot.capture_observation() low_dim = raw_obs["low_dim"] for comp in self.robot.components: obs["qpos"].extend(low_dim[f"observation/{comp.value}/joint_position"]) - for camera in self.robot.cameras: + for camera in self.robot.cameras_goal: assert camera not in obs["images"], f"Duplicate camera name: {camera}" obs["images"][camera.value] = raw_obs[f"observation.images.{camera.value}"] diff --git a/robots/airbots/airbot_mmk/airbot_com_mmk2.py b/robots/airbots/airbot_mmk/airbot_com_mmk2.py index 1f65bae..c8033fe 100644 --- a/robots/airbots/airbot_mmk/airbot_com_mmk2.py +++ b/robots/airbots/airbot_mmk/airbot_com_mmk2.py @@ -16,7 +16,7 @@ TrackingParams, ForwardPositionParams, ) -from typing import Optional, Dict, List, Tuple +from typing import Optional, Dict, List, Tuple, Any from dataclasses import dataclass, replace, field import time import logging @@ -33,7 +33,7 @@ class AIRBOTMMK2Config(object): ip: str = "192.168.11.200" port: int = 50055 default_action: Optional[List[float]] = None - cameras: Dict[str, List[str]] = field(default_factory=lambda: {}) + cameras: Dict[str, Dict[str, Any]] = field(default_factory=dict) components: List[str] = field( default_factory=lambda: [ MMK2Components.LEFT_ARM.value, @@ -57,46 +57,45 @@ def __init__(self, config: Optional[AIRBOTMMK2Config] = None, **kwargs) -> None: self.config.domain_id, ) self.joint_names = {} - self.cameras: Dict[MMK2Components, str] = {} + self.cameras_goal: Dict[MMK2Components, List[ImageTypes]] = {} + self.cameras_cfg: Dict[MMK2Components, Dict[str, str]] = {} + self.cameras = self.cameras_goal.keys() self.components: Dict[MMK2Components, ComponentTypes] = {} - all_joint_names = JointNames() self.joint_num = 0 - for k, types in self.config.cameras.items(): - self.cameras[MMK2Components(k)] = [ImageTypes(v) for v in types] + for k, cfg in self.config.cameras.items(): + comp = MMK2Components(k) + types = {ImageTypes(v) for v in cfg.pop("image_types")} + self.cameras_goal[comp] = types + if types != {ImageTypes.COLOR}: + cfg["enable_depth"] = "true" + if ImageTypes.ALIGNED_DEPTH_TO_COLOR in types: + cfg["align_depth.enable"] = "true" + self.cameras_cfg[comp] = cfg for comp_str in self.config.components: comp = MMK2Components(comp_str) # TODO: get the type info from SDK self.components[comp] = ComponentTypes.UNKNOWN - names = all_joint_names.__dict__[comp_str] + names = JointNames[comp.name].value self.joint_names[comp] = names self.joint_num += len(names) logger.info(f"Components: {self.components}") logger.info(f"Joint numbers: {self.joint_num}") - self.robot.enable_resources( - { - comp: { - "rgb_camera.color_profile": "640,480,30", - "enable_depth": "false", - } - for comp in self.cameras - } - ) + logger.info(f"enable resources cfg: {self.cameras_cfg}") + self.robot.enable_resources(self.cameras_cfg) # use stream to get images # self.robot.enable_stream(self.robot.get_image, self.cameras) + comp_action_topic = {} if self.config.demonstrate: - comp_action_topic = { - comp: TopicNames.tracking.format(component=comp.value) - for comp in MMK2ComponentsGroup.ARMS - } - comp_action_topic.update( - { - comp: TopicNames.controller_command.format( + for comp in self.components: + if comp in MMK2ComponentsGroup.ARMS: + comp_action_topic[comp] = TopicNames.tracking.format( + component=comp.value + ) + elif comp in MMK2ComponentsGroup.HEAD_SPINE: + comp_action_topic[comp] = TopicNames.controller_command.format( component=comp.value, controller=ControllerTypes.FORWARD_POSITION.value, ) - for comp in MMK2ComponentsGroup.HEAD_SPINE - } - ) self.robot.listen_to(list(comp_action_topic.values())) self._comp_action_topic = comp_action_topic self.logs = {} @@ -111,23 +110,16 @@ def __init__(self, config: Optional[AIRBOTMMK2Config] = None, **kwargs) -> None: self.reset() def _move_by_traj(self, goal: dict): - # goal.update( - # { - # MMK2Components.HEAD: JointState(position=[0, -1.0]), - # MMK2Components.SPINE: JointState(position=[0.15]), - # } - # ) if self.config.demonstrate: # TODO: since the arms and eefs are controlled by the teleop bag for comp in MMK2ComponentsGroup.ARMS_EEFS: - goal.pop(comp) + goal.pop(comp, None) if goal: # start = time.time() # logger.info(f"Move by trajectory") self.robot.set_goal(goal, TrajectoryParams()) # logger.info(f"Move by trajectory time: {time.time() - start}") self.robot.set_goal(goal, ForwardPositionParams()) - return goal def reset(self, sleep_time=0): @@ -211,7 +203,7 @@ def _capture_images(self) -> Tuple[Dict[str, bytes], Dict[str, Time]]: images = {} img_stamps: Dict[MMK2Components, Time] = {} before_camread_t = time.perf_counter() - comp_images = self.robot.get_image(self.cameras) + comp_images = self.robot.get_image(self.cameras_goal) for comp, image in comp_images.items(): # TODO: now only support for color image images[comp.value] = image.data[ImageTypes.COLOR] diff --git a/robots/airbots/airbot_mmk/airbot_com_mmk2_bson.py b/robots/airbots/airbot_mmk/airbot_com_mmk2_bson.py index 61df8f4..fa759fc 100644 --- a/robots/airbots/airbot_mmk/airbot_com_mmk2_bson.py +++ b/robots/airbots/airbot_mmk/airbot_com_mmk2_bson.py @@ -115,13 +115,11 @@ def get_low_dim_data(self): ) if self.config.demonstrate: if comp in MMK2ComponentsGroup.ARMS: - arm_jn = JointNames().__dict__[comp.value] + arm_jn = JointNames[comp.name].value comp_eef = comp.value + "_eef" - eef_jn = JointNames().__dict__[comp_eef] + eef_jn = JointNames[MMK2Components(comp_eef).name].value js = self.robot.get_listened(self._comp_action_topic[comp]) - assert ( - js is not None - ), "The AIRBOT MMK2 should be in bag teleopration mode." + assert js is not None, "The robot should be in teleopration mode." jq = self.robot.get_joint_values_by_names(js, arm_jn + eef_jn) data.update( self._get_joint_state( @@ -153,6 +151,56 @@ def capture_observation(self): data.update(self._get_image(name, img_stamps[name], img)) return data + @property + def bson_dict(self): + bson_dict = { + "id": "734ad1c8-66ee-4479-b3cb-41d16c9b2e22", + "timestamp": 1734076528859, + "metadata": { + "driver_version": "1.0.0", + "operator": "manual", + "station_id": "3784D4BA-87AF-47E7-B86D-42CA1904AA77", + "task": "example", + "version": "1.2.1", + "topics": {}, + }, + "data": {}, + } + topics = bson_dict["metadata"]["topics"] + for comp in self.components: + for tp in ["action", "observation"]: + topics[f"/{tp}/{comp.value}/joint_state"] = { + "description": "", + "type": "jointstate", + "sn": "", + "firmware_version": "0.0.0", + } + if comp in MMK2ComponentsGroup.ARMS: + topics[f"/observation/{comp.value}/pose"] = { + "description": "", + "type": "pose", + } + for camera in self.cameras: + topics[f"/images/{camera.value}"] = { + "description": "DSJ-2062-309", + "type": "image", + "width": 640, + "height": 480, + "encoding": "H264", + "distortion_model": None, + "distortion_params": None, + "intrinsics": None, + "fov": 120.0, + "start_time": 1733377253041, + } + # "/action/eef/pose": { + # "description": "", + # "type": "jointstate", + # "sn": "", + # "firmware_version": "0.0.0", + # }, + return bson_dict + def main(): robot = AIRBOTMMK2() From ae3754b8ec02efbb5d84b2e859100f3dd15c0225 Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Wed, 26 Mar 2025 09:29:29 +0800 Subject: [PATCH 02/27] show episode info --- data_process/raw_to_hdf5.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/data_process/raw_to_hdf5.py b/data_process/raw_to_hdf5.py index 413d71f..2c2fc28 100644 --- a/data_process/raw_to_hdf5.py +++ b/data_process/raw_to_hdf5.py @@ -96,6 +96,9 @@ def load_raw_mujoco_data(raw_dir, downsampling=0): episode_lens = [] for low_d in low_dim_data.values(): episode_lens.append(len(list(low_d.values())[0])) +print( + f"min episode length: {min(episode_lens)}, max episode length: {max(episode_lens)}" +) max_pad_length = max(episode_lens) if padding else None print(f"Episode flatten keys: {list(low_dim_data.values())[0].keys()}") @@ -134,4 +137,4 @@ def save_one(index, ep_name): # # save one data # index = 0 # ep_name = episode_names[index] -# save_one(index, ep_name) \ No newline at end of file +# save_one(index, ep_name) From 587e51d8f2f821be8099367437cf920e92b0e26a Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Thu, 27 Mar 2025 10:13:23 +0800 Subject: [PATCH 03/27] change tok_ptk to tok and add base component --- ...k_ptk_demonstration.yaml => airbot_tok_demonstration.yaml} | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) rename configurations/basic_configs/example/robot/airbots/tok/{airbot_tok_ptk_demonstration.yaml => airbot_tok_demonstration.yaml} (89%) diff --git a/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk_demonstration.yaml b/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_demonstration.yaml similarity index 89% rename from configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk_demonstration.yaml rename to configurations/basic_configs/example/robot/airbots/tok/airbot_tok_demonstration.yaml index 2d6e670..8a45193 100644 --- a/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk_demonstration.yaml +++ b/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_demonstration.yaml @@ -3,8 +3,8 @@ _target_: robots.airbots.airbot_mmk.airbot_com_mmk2_bson.AIRBOTMMK2 ip: "localhost" # NOTE: If you want to change the order of each part, be careful to change the order of each observation and action in the data conversion script simultaneously -components: ["left_arm", "left_arm_eef"] -# components: ["left_arm", "left_arm_eef", "right_arm", "right_arm_eef"] +# components: ["left_arm", "left_arm_eef"] +components: ["left_arm", "left_arm_eef", "right_arm", "right_arm_eef", "base"] # USB cameras (only color image types) cameras: From f053df9818be3c00e5b6aab55bcc7ba79fd90197 Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Thu, 27 Mar 2025 11:25:49 +0800 Subject: [PATCH 04/27] fix action size --- .../airbots/tok/airbot_tok_demonstration.yaml | 21 +++++++++++++-- .../robot/airbots/tok/airbot_tok_ptk.yaml | 26 +++---------------- 2 files changed, 22 insertions(+), 25 deletions(-) diff --git a/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_demonstration.yaml b/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_demonstration.yaml index 8a45193..fda0795 100644 --- a/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_demonstration.yaml +++ b/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_demonstration.yaml @@ -39,5 +39,22 @@ cameras: demonstrate: true default_action: - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - # [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + # [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] diff --git a/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk.yaml b/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk.yaml index b5742d9..18ed9bc 100644 --- a/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk.yaml +++ b/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk.yaml @@ -6,26 +6,6 @@ ip: "localhost" components: ["left_arm", "left_arm_eef", "right_arm", "right_arm_eef"] cameras: - head_camera: ["color"] - # left_camera: ["color"] - # right_camera: ["color"] - -demonstrate: true - -default_action: - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - ] + # head_camera: ["color"] + left_camera: ["color"] + right_camera: ["color"] From 5015ee1d5003ee08ea21cb0209b283d13e920294 Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Thu, 27 Mar 2025 11:41:55 +0800 Subject: [PATCH 05/27] improve camera configs --- .../airbots/tok/airbot_tok_demonstration.yaml | 11 ++++- .../robot/airbots/tok/airbot_tok_ptk.yaml | 40 +++++++++++++++++-- 2 files changed, 47 insertions(+), 4 deletions(-) diff --git a/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_demonstration.yaml b/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_demonstration.yaml index fda0795..d45f02d 100644 --- a/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_demonstration.yaml +++ b/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_demonstration.yaml @@ -12,29 +12,38 @@ cameras: image_types: ["color"] camera_type: "USB" video_device: "/dev/video0" + image_width: "640" + image_height: "480" # left_camera: # image_types: ["color"] # camera_type: "USB" # video_device: "/dev/video2" + # image_width: "640" + # image_height: "480" # right_camera: # image_types: ["color"] # camera_type: "USB" # video_device: "/dev/video4" + # image_width: "640" + # image_height: "480" -# RealSense +# # RealSense # cameras: # head_camera: # image_types: ["color"] # camera_type: "REALSENSE" # serial_no: "'123456'" +# rgb_camera.color_profile: "640,480,30" # left_camera: # image_types: ["color"] # camera_type: "REALSENSE" # serial_no: "'123456'" +# rgb_camera.color_profile: "640,480,30" # right_camera: # image_types: ["color"] # camera_type: "REALSENSE" # serial_no: "'123456'" +# rgb_camera.color_profile: "640,480,30" demonstrate: true diff --git a/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk.yaml b/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk.yaml index 18ed9bc..5989fd4 100644 --- a/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk.yaml +++ b/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk.yaml @@ -5,7 +5,41 @@ ip: "localhost" # NOTE: If you want to change the order of each part, be careful to change the order of each observation and action in the data conversion script simultaneously components: ["left_arm", "left_arm_eef", "right_arm", "right_arm_eef"] +# USB cameras (only color image types) cameras: - # head_camera: ["color"] - left_camera: ["color"] - right_camera: ["color"] + head_camera: + image_types: ["color"] + camera_type: "USB" + video_device: "/dev/video0" + image_width: "640" + image_height: "480" + # left_camera: + # image_types: ["color"] + # camera_type: "USB" + # video_device: "/dev/video2" + # image_width: "640" + # image_height: "480" + # right_camera: + # image_types: ["color"] + # camera_type: "USB" + # video_device: "/dev/video4" + # image_width: "640" + # image_height: "480" + +# # RealSense +# cameras: +# head_camera: +# image_types: ["color"] +# camera_type: "REALSENSE" +# serial_no: "'123456'" +# rgb_camera.color_profile: "640,480,30" +# left_camera: +# image_types: ["color"] +# camera_type: "REALSENSE" +# serial_no: "'123456'" +# rgb_camera.color_profile: "640,480,30" +# right_camera: +# image_types: ["color"] +# camera_type: "REALSENSE" +# serial_no: "'123456'" +# rgb_camera.color_profile: "640,480,30" From 5ec91276b9a066e55779f2aef81b79fa36f44885 Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Thu, 27 Mar 2025 11:43:02 +0800 Subject: [PATCH 06/27] fix eval config --- .../basic_configs/example/robot/airbots/tok/airbot_tok_ptk.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk.yaml b/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk.yaml index 5989fd4..4fa52c7 100644 --- a/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk.yaml +++ b/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk.yaml @@ -1,4 +1,4 @@ -_target_: robots.airbots.airbot_mmk.airbot_com_mmk2_bson.AIRBOTMMK2 +_target_: robots.airbots.airbot_mmk.airbot_com_mmk2.AIRBOTMMK2 ip: "localhost" From f7b147035b3f13395e5d19e27a4859b2d52e58d8 Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Thu, 27 Mar 2025 14:24:22 +0800 Subject: [PATCH 07/27] udpate meta data (image resolution --- .../robot/airbots/tok/airbot_tok_ptk.yaml | 2 +- control_robot_bson.py | 2 ++ .../airbot_mmk/airbot_com_mmk2_bson.py | 21 ++++++++++++++----- robots/common.py | 1 + 4 files changed, 20 insertions(+), 6 deletions(-) diff --git a/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk.yaml b/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk.yaml index 4fa52c7..5989fd4 100644 --- a/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk.yaml +++ b/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk.yaml @@ -1,4 +1,4 @@ -_target_: robots.airbots.airbot_mmk.airbot_com_mmk2.AIRBOTMMK2 +_target_: robots.airbots.airbot_mmk.airbot_com_mmk2_bson.AIRBOTMMK2 ip: "localhost" diff --git a/control_robot_bson.py b/control_robot_bson.py index 5b038c1..6e4c8aa 100644 --- a/control_robot_bson.py +++ b/control_robot_bson.py @@ -6,6 +6,7 @@ """ from habitats.common.robot_devices.cameras.utils import prepare_cv2_imshow + prepare_cv2_imshow() import argparse @@ -571,6 +572,7 @@ def stop_recording(self): start_episode_t = time.perf_counter() # Record one episode bson_dict: Dict[str, Dict[str, list]] = robot.bson_dict + robot.update_data_meta(bson_dict, robot.capture_observation()) while timestamp < episode_time_s: start_loop_t = time.perf_counter() diff --git a/robots/airbots/airbot_mmk/airbot_com_mmk2_bson.py b/robots/airbots/airbot_mmk/airbot_com_mmk2_bson.py index fa759fc..cb348fc 100644 --- a/robots/airbots/airbot_mmk/airbot_com_mmk2_bson.py +++ b/robots/airbots/airbot_mmk/airbot_com_mmk2_bson.py @@ -103,11 +103,6 @@ def get_low_dim_data(self): joint_pos = [base_pose.x, base_pose.y, base_pose.theta] joint_vel = [base_vel.x, base_vel.y, base_vel.omega] joint_eff = [0.0, 0.0, 0.0] - data.update( - self._get_joint_state( - "action", comp.value, stamp, joint_pos, joint_vel, joint_eff - ) - ) data.update( self._get_joint_state( "observation", comp.value, stamp, joint_pos, joint_vel, joint_eff @@ -131,6 +126,13 @@ def get_low_dim_data(self): "action", comp_eef, js.header.stamp, jq[-1:] ) ) + elif comp == MMK2Components.BASE: + # TODO: now action and observation are the same for base + data.update( + self._get_joint_state( + "action", comp.value, stamp, joint_pos, joint_vel, joint_eff + ) + ) elif comp in MMK2ComponentsGroup.HEAD_SPINE: result = self.robot.get_listened(self._comp_action_topic[comp]) assert ( @@ -151,6 +153,15 @@ def capture_observation(self): data.update(self._get_image(name, img_stamps[name], img)) return data + def update_data_meta(self, bson_dict: dict, observation: dict): + topics = bson_dict["metadata"]["topics"] + for camera in self.cameras: + image = observation[f"/images/{camera.value}"] + image_meta = topics[f"/images/{camera.value}"] + h, w = image.shape[:2] + image_meta["width"] = w + image_meta["height"] = h + @property def bson_dict(self): bson_dict = { diff --git a/robots/common.py b/robots/common.py index 09f693c..0f00d41 100644 --- a/robots/common.py +++ b/robots/common.py @@ -38,6 +38,7 @@ def enter_servo_mode(self): ... def enter_traj_mode(self): ... def get_low_dim_data(self): ... def capture_observation(self): ... + def update_data_meta(self, bson_dict: dict, observation:dict): ... def send_action(self, action): ... def reset(self): ... def exit(self): ... From dd4c6b5bcd3d2ce6aefaeb1f2b7f48e2881ccc4b Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Thu, 27 Mar 2025 14:41:23 +0800 Subject: [PATCH 08/27] fix get obs image --- robots/airbots/airbot_mmk/airbot_com_mmk2_bson.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robots/airbots/airbot_mmk/airbot_com_mmk2_bson.py b/robots/airbots/airbot_mmk/airbot_com_mmk2_bson.py index cb348fc..dbcfaa4 100644 --- a/robots/airbots/airbot_mmk/airbot_com_mmk2_bson.py +++ b/robots/airbots/airbot_mmk/airbot_com_mmk2_bson.py @@ -156,7 +156,7 @@ def capture_observation(self): def update_data_meta(self, bson_dict: dict, observation: dict): topics = bson_dict["metadata"]["topics"] for camera in self.cameras: - image = observation[f"/images/{camera.value}"] + image = observation[f"/images/{camera.value}"]["data"] image_meta = topics[f"/images/{camera.value}"] h, w = image.shape[:2] image_meta["width"] = w From 5237f95c5c0bcd065e9c902fe1f1fdddb172ca92 Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Thu, 27 Mar 2025 14:50:07 +0800 Subject: [PATCH 09/27] add tok ptk mode support --- data_process/bson_to_hdf5.py | 17 +++++ data_process/mmk2_to_hdf5.py | 134 ----------------------------------- 2 files changed, 17 insertions(+), 134 deletions(-) delete mode 100644 data_process/mmk2_to_hdf5.py diff --git a/data_process/bson_to_hdf5.py b/data_process/bson_to_hdf5.py index 93c7ade..01a0ad5 100644 --- a/data_process/bson_to_hdf5.py +++ b/data_process/bson_to_hdf5.py @@ -61,6 +61,23 @@ f"/images/{raw_name}": f"/observations/images/{raw_name}" for raw_name in camera_names } +elif mode in ["tok", "ptk"]: + obs_keys_low_dim = ( + "/observation/left_arm/joint_state", + "/observation/left_arm_eef/joint_state", + "/observation/right_arm/joint_state", + "/observation/right_arm_eef/joint_state", + ) + act_keys = ( + "/action/left_arm/joint_state", + "/action/left_arm_eef/joint_state", + "/action/right_arm/joint_state", + "/action/right_arm_eef/joint_state", + ) + name_converter = { + f"/images/{raw_name}": f"/observations/images/{raw_name}" + for raw_name in camera_names + } else: raise ValueError(f"mode {mode} not supported") image_keys = [f"/images/{name}" for name in args.camera_names] diff --git a/data_process/mmk2_to_hdf5.py b/data_process/mmk2_to_hdf5.py deleted file mode 100644 index 7129b95..0000000 --- a/data_process/mmk2_to_hdf5.py +++ /dev/null @@ -1,134 +0,0 @@ -import sys, os - -sys.path.append(os.path.join(os.path.dirname(__file__), "..")) - -import data_process.convert_all as crd -import argparse - -parser = argparse.ArgumentParser(description="Convert mmk2 data to hdf5") - -parser.add_argument("--raw_dir", type=str, default="data/raw", help="input directory") -parser.add_argument("--output", type=str, default="data/hdf5", help="output directory") -parser.add_argument( - "-tn", "--task_name", type=str, default="example_task", help="task name" -) -parser.add_argument( - "--cameras", type=str, nargs="+", default=["0"], help="camera names" -) -parser.add_argument("-pad", "--padding", action="store_true", help="pad the hdf5 data") - -args = parser.parse_args() - -# get all low_dim data (head&spine velocity control) -task_name = args.task_name -raw_root_dir = args.raw_dir -raw_dir = f"{raw_root_dir}/{task_name}" -data = crd.raw_to_dict( - raw_dir, - ["low_dim.json"], - video_file_names=None, - flatten_mode="hdf5", - concatenater={ - "/observations/qpos": ( - "/observation/arm/left/joint_position", - "/observation/eef/left/joint_position", - "/observation/arm/right/joint_position", - "/observation/eef/right/joint_position", - ), - "/action": ( - "/action/arm/left/joint_position", - "/action/eef/left/joint_position", - "/action/arm/right/joint_position", - "/action/eef/right/joint_position", - ), - }, - key_filter=[ - "/observation/ts_diff_with_head_color_img", - "/observation/arm/left/joint_velocity", - "/observation/arm/right/joint_velocity", - "/observation/arm/left/joint_effort", - "/observation/arm/right/joint_effort", - "/observation/eef/left/joint_velocity", - "/observation/eef/right/joint_velocity", - "/observation/eef/left/joint_effort", - "/observation/eef/right/joint_effort", - "/observation/head/joint_position", - "/observation/head/joint_velocity", - "/observation/head/joint_effort", - "/observation/spine/joint_position", - "/observation/spine/joint_velocity", - "/observation/joint_states/time", - "/observation/time", - "/action/time", - "/action/arm/left/time", - "/action/arm/right/time", - "/action/head/color/time", - "/action/head/joint_position", - "/action/spine/joint_position", - "/action/base/velocity", - # "/action/head/joint_velocity", - # "/action/spine/joint_velocity" - ], -) - -import os -import cv2 - -# merge high_dim data and save -raw_dir -names = args.cameras -video_names = [f"{name}.mp4" for name in names] -target_root_dir = args.output -target_dir = f"{target_root_dir}/{task_name}" -low_dim_data = data -name_converter = {names[i]: f"/observations/images/{i}" for i in range(len(names))} -target_namer = lambda i: f"episode_{i}.hdf5" - -compresser = crd.Compresser("jpg", [int(cv2.IMWRITE_JPEG_QUALITY), 50], True) - -os.makedirs(target_dir, exist_ok=True) - -# get max episode length -episode_lens = [] -for key, low_d in low_dim_data.items(): - length = len(list(low_d.values())[0]) - episode_lens.append(length) - # if length < 200: - # print(f"{key} has length {length}") - -max_pad_length = max(episode_lens) if args.padding else None - -# save all data -episode_names = list(low_dim_data.keys()) -print(f"Episode lengths: {episode_lens}") -print(f"Max episode length: {max_pad_length}") -print(f"All episodes: {episode_names}") -print(f"episode number: {len(episode_names)}") -downsampling = 0 - - -def save_one(index, ep_name): - crd.merge_video_and_save( - low_dim_data[ep_name], - f"{raw_dir}/{ep_name}", - video_names, - crd.save_dict_to_hdf5, - name_converter, - compresser, - f"{target_dir}/" + target_namer(index), - max_pad_length, - downsampling, - ) - data.pop(ep_name) - - -# save all -from concurrent.futures import ThreadPoolExecutor - -futures = [] -with ThreadPoolExecutor(max_workers=25) as executor: - for index, ep_name in enumerate(episode_names): - # silent execution, no print - futures.append(executor.submit(save_one, index, ep_name)) - -print(f"All data saved to {target_dir}") From 4d662add018ef91e2963d9d8bbcd8760b5b10f5f Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Thu, 27 Mar 2025 14:59:47 +0800 Subject: [PATCH 10/27] fix pose filt for ptk and tok --- data_process/bson_to_hdf5.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/data_process/bson_to_hdf5.py b/data_process/bson_to_hdf5.py index 01a0ad5..9230050 100644 --- a/data_process/bson_to_hdf5.py +++ b/data_process/bson_to_hdf5.py @@ -99,7 +99,7 @@ } ) -if mode == "mmk2": +if mode in ["mmk2", "tok", "ptk"]: key_filter = [ "/observation/left_arm/pose", "/observation/right_arm/pose", From ca916dd94cc3a4af517e7cfc44d8cf908e73ac0e Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Thu, 27 Mar 2025 15:00:43 +0800 Subject: [PATCH 11/27] fix episodes finding for ptk and tok --- data_process/bson_to_hdf5.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/data_process/bson_to_hdf5.py b/data_process/bson_to_hdf5.py index 9230050..d1c2dd9 100644 --- a/data_process/bson_to_hdf5.py +++ b/data_process/bson_to_hdf5.py @@ -138,7 +138,7 @@ os.makedirs(target_dir, exist_ok=True) print(f"Try to find all episode files in {task_dir}...") -if mode == "mmk2": +if mode in ["mmk2", "tok", "ptk"]: episode_names = crd.get_files_name_by_suffix(task_dir, ".bson") elif mode == "play": episode_names = [f"{fd}/data.bson" for fd in os.listdir(task_dir)] From 061dda21923d85045d583d580e7936b41e0722a4 Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Thu, 27 Mar 2025 15:08:10 +0800 Subject: [PATCH 12/27] add base process --- data_process/bson_to_hdf5.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/data_process/bson_to_hdf5.py b/data_process/bson_to_hdf5.py index d1c2dd9..09c3246 100644 --- a/data_process/bson_to_hdf5.py +++ b/data_process/bson_to_hdf5.py @@ -48,6 +48,7 @@ "/observation/right_arm_eef/joint_state", "/observation/head/joint_state", "/observation/spine/joint_state", + "/observation/base/joint_state", ) act_keys = ( "/action/left_arm/joint_state", @@ -56,6 +57,7 @@ "/action/right_arm_eef/joint_state", "/action/head/joint_state", "/action/spine/joint_state", + "/action/base/joint_state", ) name_converter = { f"/images/{raw_name}": f"/observations/images/{raw_name}" @@ -67,12 +69,14 @@ "/observation/left_arm_eef/joint_state", "/observation/right_arm/joint_state", "/observation/right_arm_eef/joint_state", + "/observation/base/joint_state", ) act_keys = ( "/action/left_arm/joint_state", "/action/left_arm_eef/joint_state", "/action/right_arm/joint_state", "/action/right_arm_eef/joint_state", + "/action/base/joint_state", ) name_converter = { f"/images/{raw_name}": f"/observations/images/{raw_name}" @@ -103,6 +107,8 @@ key_filter = [ "/observation/left_arm/pose", "/observation/right_arm/pose", + "/observation/base/joint_state", + # "/action/base/joint_state", # "action/eef/pose", # "/time", ] From db2093689098aa5b812916cc6b2b57135bfaadae Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Thu, 27 Mar 2025 15:09:54 +0800 Subject: [PATCH 13/27] fix base filt --- data_process/bson_to_hdf5.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/data_process/bson_to_hdf5.py b/data_process/bson_to_hdf5.py index 09c3246..e2e8423 100644 --- a/data_process/bson_to_hdf5.py +++ b/data_process/bson_to_hdf5.py @@ -107,7 +107,7 @@ key_filter = [ "/observation/left_arm/pose", "/observation/right_arm/pose", - "/observation/base/joint_state", + # "/observation/base/joint_state", # "/action/base/joint_state", # "action/eef/pose", # "/time", From 80e9708ac68e25685723db953a48b2a16f1bb2ea Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Thu, 27 Mar 2025 15:18:25 +0800 Subject: [PATCH 14/27] use common name converter --- data_process/bson_to_hdf5.py | 17 ++++------------- 1 file changed, 4 insertions(+), 13 deletions(-) diff --git a/data_process/bson_to_hdf5.py b/data_process/bson_to_hdf5.py index e2e8423..2124b2c 100644 --- a/data_process/bson_to_hdf5.py +++ b/data_process/bson_to_hdf5.py @@ -30,9 +30,11 @@ assert os.path.exists(task_dir), f"task_dir {task_dir} not exists" name_converter = { - f"/images/{raw_name}": f"/observations/images/{i}" - for i, raw_name in enumerate(camera_names) + f"/images/{raw_name}": f"/observations/images/{raw_name}" + for raw_name in camera_names } +print(f"name_converter: {name_converter}") +image_keys = [f"/images/{name}" for name in args.camera_names] if mode == "play": obs_keys_low_dim = ( @@ -59,10 +61,6 @@ "/action/spine/joint_state", "/action/base/joint_state", ) - name_converter = { - f"/images/{raw_name}": f"/observations/images/{raw_name}" - for raw_name in camera_names - } elif mode in ["tok", "ptk"]: obs_keys_low_dim = ( "/observation/left_arm/joint_state", @@ -78,15 +76,8 @@ "/action/right_arm_eef/joint_state", "/action/base/joint_state", ) - name_converter = { - f"/images/{raw_name}": f"/observations/images/{raw_name}" - for raw_name in camera_names - } else: raise ValueError(f"mode {mode} not supported") -image_keys = [f"/images/{name}" for name in args.camera_names] - -print(f"name_converter: {name_converter}") pre_process = { key: crd.Compresser("jpg", [int(cv2.IMWRITE_JPEG_QUALITY), 50], True).compress From aea786dec22ed08aed378ec68d13d12f874c6541 Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Thu, 27 Mar 2025 16:56:18 +0800 Subject: [PATCH 15/27] disable dim check for tok ptk to ignore base action --- .../example/robot/airbots/tok/airbot_tok_ptk.yaml | 2 ++ envs/airbot_com_mmk_env.py | 12 ++++++++---- robots/airbots/airbot_mmk/airbot_com_mmk2.py | 3 +++ 3 files changed, 13 insertions(+), 4 deletions(-) diff --git a/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk.yaml b/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk.yaml index 5989fd4..c3abd8e 100644 --- a/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk.yaml +++ b/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk.yaml @@ -43,3 +43,5 @@ cameras: # camera_type: "REALSENSE" # serial_no: "'123456'" # rgb_camera.color_profile: "640,480,30" + +check_dim: false \ No newline at end of file diff --git a/envs/airbot_com_mmk_env.py b/envs/airbot_com_mmk_env.py index 1bc7460..f54dfae 100644 --- a/envs/airbot_com_mmk_env.py +++ b/envs/airbot_com_mmk_env.py @@ -12,9 +12,12 @@ def __init__(self, config_path: str): self._all_joints_num = self.robot.joint_num def set_reset_position(self, reset_position): - assert ( - len(reset_position) == self._all_joints_num - ), f"Expected {self._all_joints_num} joints, got {len(reset_position)}" + if not len(reset_position) == self._all_joints_num: + des = f"Expected {self._all_joints_num} joints, got {len(reset_position)}" + if self.robot.config.check_dim: + raise ValueError(des) + else: + print(f"Warning: {des}") self.robot.config.default_action = reset_position def reset(self, sleep_time=0): @@ -46,11 +49,12 @@ def step( sleep_time=0, get_obs=True, ): + # TODO: require the arms to be first components joint_limits = ( (-3.09, 2.04), (-2.92, 0.12), (-0.04, 3.09), - (-2.95, 2.95), # (-3.1, 3.1) + (-2.95, 2.95), # (-3.1, 3.1) (-1.9, 1.9), # (-1.08, 1.08), (-2.90, 2.90), # (-3.0, 3.0) (0, 1), diff --git a/robots/airbots/airbot_mmk/airbot_com_mmk2.py b/robots/airbots/airbot_mmk/airbot_com_mmk2.py index c8033fe..bcc5280 100644 --- a/robots/airbots/airbot_mmk/airbot_com_mmk2.py +++ b/robots/airbots/airbot_mmk/airbot_com_mmk2.py @@ -15,6 +15,8 @@ MoveServoParams, TrackingParams, ForwardPositionParams, + Pose3D, + Twist3D, ) from typing import Optional, Dict, List, Tuple, Any from dataclasses import dataclass, replace, field @@ -43,6 +45,7 @@ class AIRBOTMMK2Config(object): ] ) demonstrate: bool = False + check_dim: bool = True class AIRBOTMMK2(object): From 52f0a5ca5c619036a32c98a34c08421a00181d52 Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Sat, 29 Mar 2025 20:53:50 +0800 Subject: [PATCH 16/27] fix not check dim check --- robots/airbots/airbot_mmk/airbot_com_mmk2.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/robots/airbots/airbot_mmk/airbot_com_mmk2.py b/robots/airbots/airbot_mmk/airbot_com_mmk2.py index bcc5280..3766899 100644 --- a/robots/airbots/airbot_mmk/airbot_com_mmk2.py +++ b/robots/airbots/airbot_mmk/airbot_com_mmk2.py @@ -259,7 +259,8 @@ def _action_check(self, action): ), f"Invalid action {action} with length: {len(action)}" def _action_to_goal(self, action) -> Dict[MMK2Components, JointState]: - self._action_check(action) + if self.config.check_dim: + self._action_check(action) goal = {} j_cnt = 0 for comp in self.components: From f2d22d85b47973826352ec96ac90d785bf84e563 Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Sat, 29 Mar 2025 21:01:34 +0800 Subject: [PATCH 17/27] remove lowdim level in obs data --- envs/airbot_com_mmk_env.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/envs/airbot_com_mmk_env.py b/envs/airbot_com_mmk_env.py index f54dfae..ba5abb2 100644 --- a/envs/airbot_com_mmk_env.py +++ b/envs/airbot_com_mmk_env.py @@ -29,9 +29,8 @@ def _get_obs(self): obs["qpos"] = [] obs["images"] = {} raw_obs = self.robot.capture_observation() - low_dim = raw_obs["low_dim"] for comp in self.robot.components: - obs["qpos"].extend(low_dim[f"observation/{comp.value}/joint_position"]) + obs["qpos"].extend(raw_obs[f"observation/{comp.value}/joint_position"]) for camera in self.robot.cameras_goal: assert camera not in obs["images"], f"Duplicate camera name: {camera}" obs["images"][camera.value] = raw_obs[f"observation.images.{camera.value}"] From 8fdf8c84defc7bac5fc8fc94614c013df8f43cf4 Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Sat, 29 Mar 2025 21:11:08 +0800 Subject: [PATCH 18/27] fix get com bson data --- envs/airbot_com_mmk_env.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/envs/airbot_com_mmk_env.py b/envs/airbot_com_mmk_env.py index ba5abb2..7e24fc0 100644 --- a/envs/airbot_com_mmk_env.py +++ b/envs/airbot_com_mmk_env.py @@ -30,10 +30,10 @@ def _get_obs(self): obs["images"] = {} raw_obs = self.robot.capture_observation() for comp in self.robot.components: - obs["qpos"].extend(raw_obs[f"observation/{comp.value}/joint_position"]) + obs["qpos"].extend(raw_obs[f"/observation/{comp.value}/joint_state"]["data"]["pos"]) for camera in self.robot.cameras_goal: assert camera not in obs["images"], f"Duplicate camera name: {camera}" - obs["images"][camera.value] = raw_obs[f"observation.images.{camera.value}"] + obs["images"][camera.value] = raw_obs[f"/images/{camera.value}"]["data"] return dm_env.TimeStep( step_type=dm_env.StepType.FIRST, From a30f28c84a5d532ba24e5f8f4c5efb65b68f5731 Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Sat, 29 Mar 2025 21:25:58 +0800 Subject: [PATCH 19/27] add low dim to action in bson --- robots/airbots/airbot_mmk/airbot_com_mmk2.py | 1 + robots/airbots/airbot_mmk/airbot_com_mmk2_bson.py | 6 ++++++ 2 files changed, 7 insertions(+) diff --git a/robots/airbots/airbot_mmk/airbot_com_mmk2.py b/robots/airbots/airbot_mmk/airbot_com_mmk2.py index 3766899..27f46d1 100644 --- a/robots/airbots/airbot_mmk/airbot_com_mmk2.py +++ b/robots/airbots/airbot_mmk/airbot_com_mmk2.py @@ -123,6 +123,7 @@ def _move_by_traj(self, goal: dict): self.robot.set_goal(goal, TrajectoryParams()) # logger.info(f"Move by trajectory time: {time.time() - start}") self.robot.set_goal(goal, ForwardPositionParams()) + logger.info(f"Move by trajectory: {goal}") return goal def reset(self, sleep_time=0): diff --git a/robots/airbots/airbot_mmk/airbot_com_mmk2_bson.py b/robots/airbots/airbot_mmk/airbot_com_mmk2_bson.py index dbcfaa4..2199a3f 100644 --- a/robots/airbots/airbot_mmk/airbot_com_mmk2_bson.py +++ b/robots/airbots/airbot_mmk/airbot_com_mmk2_bson.py @@ -162,6 +162,12 @@ def update_data_meta(self, bson_dict: dict, observation: dict): image_meta["width"] = w image_meta["height"] = h + def low_dim_to_action(self, low_dim: dict, step: int) -> list: + action = [] + for comp in self.components: + action.extend(low_dim[f"/action/{comp.value}/joint_state"]["data"][step]) + return action + @property def bson_dict(self): bson_dict = { From d7c0d2cc677f5ccfe1110be6b30409da860ff692 Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Sat, 29 Mar 2025 22:05:14 +0800 Subject: [PATCH 20/27] add ignore base action config --- .../example/robot/airbots/tok/airbot_tok_ptk.yaml | 4 ++-- robots/airbots/airbot_mmk/airbot_com_mmk2.py | 3 +++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk.yaml b/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk.yaml index c3abd8e..f18d73b 100644 --- a/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk.yaml +++ b/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_ptk.yaml @@ -3,7 +3,7 @@ _target_: robots.airbots.airbot_mmk.airbot_com_mmk2_bson.AIRBOTMMK2 ip: "localhost" # NOTE: If you want to change the order of each part, be careful to change the order of each observation and action in the data conversion script simultaneously -components: ["left_arm", "left_arm_eef", "right_arm", "right_arm_eef"] +components: ["left_arm", "left_arm_eef", "right_arm", "right_arm_eef", "base"] # USB cameras (only color image types) cameras: @@ -44,4 +44,4 @@ cameras: # serial_no: "'123456'" # rgb_camera.color_profile: "640,480,30" -check_dim: false \ No newline at end of file +ignore_base_action: true \ No newline at end of file diff --git a/robots/airbots/airbot_mmk/airbot_com_mmk2.py b/robots/airbots/airbot_mmk/airbot_com_mmk2.py index 27f46d1..d2ec4bb 100644 --- a/robots/airbots/airbot_mmk/airbot_com_mmk2.py +++ b/robots/airbots/airbot_mmk/airbot_com_mmk2.py @@ -46,6 +46,7 @@ class AIRBOTMMK2Config(object): ) demonstrate: bool = False check_dim: bool = True + ignore_base_action: bool = False class AIRBOTMMK2(object): @@ -144,6 +145,8 @@ def send_action(self, action, wait=False): # logger.info(f"Send goal: {goal}") # param = MoveServoParams(header=self.robot.get_header()) + if self.config.ignore_base_action: + goal.pop(MMK2Components.BASE, None) if self.traj_mode: self._move_by_traj(goal) else: From 944df5b9d410cea16e6d787854949d4b9a74b33f Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Sat, 29 Mar 2025 22:08:40 +0800 Subject: [PATCH 21/27] fix base joint names --- robots/airbots/airbot_mmk/airbot_com_mmk2.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/robots/airbots/airbot_mmk/airbot_com_mmk2.py b/robots/airbots/airbot_mmk/airbot_com_mmk2.py index d2ec4bb..65ed5cf 100644 --- a/robots/airbots/airbot_mmk/airbot_com_mmk2.py +++ b/robots/airbots/airbot_mmk/airbot_com_mmk2.py @@ -80,6 +80,9 @@ def __init__(self, config: Optional[AIRBOTMMK2Config] = None, **kwargs) -> None: # TODO: get the type info from SDK self.components[comp] = ComponentTypes.UNKNOWN names = JointNames[comp.name].value + if comp == MMK2Components.BASE: + # TODO: fix base control + names.append("base") self.joint_names[comp] = names self.joint_num += len(names) logger.info(f"Components: {self.components}") From 48cff916afec2f1c612a28285e053c7f4bba9755 Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Sat, 29 Mar 2025 22:09:56 +0800 Subject: [PATCH 22/27] update --- robots/airbots/airbot_mmk/airbot_com_mmk2.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robots/airbots/airbot_mmk/airbot_com_mmk2.py b/robots/airbots/airbot_mmk/airbot_com_mmk2.py index 65ed5cf..ed7d8df 100644 --- a/robots/airbots/airbot_mmk/airbot_com_mmk2.py +++ b/robots/airbots/airbot_mmk/airbot_com_mmk2.py @@ -127,7 +127,7 @@ def _move_by_traj(self, goal: dict): self.robot.set_goal(goal, TrajectoryParams()) # logger.info(f"Move by trajectory time: {time.time() - start}") self.robot.set_goal(goal, ForwardPositionParams()) - logger.info(f"Move by trajectory: {goal}") + # logger.info(f"Move by trajectory: {goal}") return goal def reset(self, sleep_time=0): From a0510348320f9e2e14a3d5dd45e15163162749d0 Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Wed, 2 Apr 2025 11:34:19 +0800 Subject: [PATCH 23/27] update limit and fix default action --- .../robot/airbots/tok/airbot_tok_demonstration.yaml | 1 + envs/airbot_com_mmk_env.py | 12 ++++++------ 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_demonstration.yaml b/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_demonstration.yaml index d45f02d..24ba41c 100644 --- a/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_demonstration.yaml +++ b/configurations/basic_configs/example/robot/airbots/tok/airbot_tok_demonstration.yaml @@ -65,5 +65,6 @@ default_action: 0.0, 0.0, 0.0, + 0.0, ] # [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] diff --git a/envs/airbot_com_mmk_env.py b/envs/airbot_com_mmk_env.py index 7e24fc0..e3e180a 100644 --- a/envs/airbot_com_mmk_env.py +++ b/envs/airbot_com_mmk_env.py @@ -50,12 +50,12 @@ def step( ): # TODO: require the arms to be first components joint_limits = ( - (-3.09, 2.04), - (-2.92, 0.12), - (-0.04, 3.09), - (-2.95, 2.95), # (-3.1, 3.1) - (-1.9, 1.9), # (-1.08, 1.08), - (-2.90, 2.90), # (-3.0, 3.0) + (-3.151, 2.09), # (-3.09, 2.04) + (-2.963, 0.181), # (-2.92, 0.12) + (-0.094, 3.161), # (-0.04, 3.09) + (-2.95, 2.95), # (-3.012, 3.012) + (-1.9, 1.9), # (-1.859, 1.859) + (-2.90, 2.90), # (-3.017, 3.017) (0, 1), ) From ff25f59164656c8daabbb354bb29a32d24c7d788 Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Wed, 9 Apr 2025 14:58:42 +0800 Subject: [PATCH 24/27] 1. fix component types 2. fix listen to topic names --- robots/airbots/airbot_mmk/airbot_com_mmk2.py | 65 ++++--- .../airbot_mmk/airbot_com_mmk2_bson.py | 22 +-- robots/airbots/airbot_mmk/airbot_mmk2.py | 62 +++---- robots/airbots/airbot_mmk/airbot_mmk2_bson.py | 10 +- robots/airbots/airbot_play/airbot_play_5.py | 173 ++++++++++++++++++ 5 files changed, 252 insertions(+), 80 deletions(-) create mode 100644 robots/airbots/airbot_play/airbot_play_5.py diff --git a/robots/airbots/airbot_mmk/airbot_com_mmk2.py b/robots/airbots/airbot_mmk/airbot_com_mmk2.py index ed7d8df..8c3c79a 100644 --- a/robots/airbots/airbot_mmk/airbot_com_mmk2.py +++ b/robots/airbots/airbot_mmk/airbot_com_mmk2.py @@ -1,10 +1,10 @@ from airbot_py.airbot_mmk2 import AirbotMMK2 from mmk2_types.types import ( - MMK2Components, + RobotComponents, JointNames, ComponentTypes, TopicNames, - MMK2ComponentsGroup, + RobotComponentsGroup, ImageTypes, ControllerTypes, ) @@ -29,7 +29,7 @@ @dataclass -class AIRBOTMMK2Config(object): +class AIRBOTRobotConfig(object): name: str = "mmk2" domain_id: int = -1 ip: str = "192.168.11.200" @@ -38,10 +38,10 @@ class AIRBOTMMK2Config(object): cameras: Dict[str, Dict[str, Any]] = field(default_factory=dict) components: List[str] = field( default_factory=lambda: [ - MMK2Components.LEFT_ARM.value, - MMK2Components.LEFT_ARM_EEF.value, - MMK2Components.RIGHT_ARM.value, - MMK2Components.RIGHT_ARM_EEF.value, + RobotComponents.LEFT_ARM.value, + RobotComponents.LEFT_ARM_EEF.value, + RobotComponents.RIGHT_ARM.value, + RobotComponents.RIGHT_ARM_EEF.value, ] ) demonstrate: bool = False @@ -50,9 +50,9 @@ class AIRBOTMMK2Config(object): class AIRBOTMMK2(object): - def __init__(self, config: Optional[AIRBOTMMK2Config] = None, **kwargs) -> None: + def __init__(self, config: Optional[AIRBOTRobotConfig] = None, **kwargs) -> None: if config is None: - config = AIRBOTMMK2Config() + config = AIRBOTRobotConfig() self.config = replace(config, **kwargs) self.robot = AirbotMMK2( self.config.ip, @@ -61,13 +61,13 @@ def __init__(self, config: Optional[AIRBOTMMK2Config] = None, **kwargs) -> None: self.config.domain_id, ) self.joint_names = {} - self.cameras_goal: Dict[MMK2Components, List[ImageTypes]] = {} - self.cameras_cfg: Dict[MMK2Components, Dict[str, str]] = {} + self.cameras_goal: Dict[RobotComponents, List[ImageTypes]] = {} + self.cameras_cfg: Dict[RobotComponents, Dict[str, str]] = {} self.cameras = self.cameras_goal.keys() - self.components: Dict[MMK2Components, ComponentTypes] = {} + self.components: Dict[RobotComponents, ComponentTypes] = {} self.joint_num = 0 for k, cfg in self.config.cameras.items(): - comp = MMK2Components(k) + comp = RobotComponents(k) types = {ImageTypes(v) for v in cfg.pop("image_types")} self.cameras_goal[comp] = types if types != {ImageTypes.COLOR}: @@ -76,11 +76,11 @@ def __init__(self, config: Optional[AIRBOTMMK2Config] = None, **kwargs) -> None: cfg["align_depth.enable"] = "true" self.cameras_cfg[comp] = cfg for comp_str in self.config.components: - comp = MMK2Components(comp_str) + comp = RobotComponents(comp_str) # TODO: get the type info from SDK self.components[comp] = ComponentTypes.UNKNOWN names = JointNames[comp.name].value - if comp == MMK2Components.BASE: + if comp == RobotComponents.BASE: # TODO: fix base control names.append("base") self.joint_names[comp] = names @@ -94,14 +94,13 @@ def __init__(self, config: Optional[AIRBOTMMK2Config] = None, **kwargs) -> None: comp_action_topic = {} if self.config.demonstrate: for comp in self.components: - if comp in MMK2ComponentsGroup.ARMS: + if comp in RobotComponentsGroup.ARMS: comp_action_topic[comp] = TopicNames.tracking.format( component=comp.value ) - elif comp in MMK2ComponentsGroup.HEAD_SPINE: + elif comp in RobotComponentsGroup.HEAD_SPINE: comp_action_topic[comp] = TopicNames.controller_command.format( - component=comp.value, - controller=ControllerTypes.FORWARD_POSITION.value, + controller=f"/{comp.value}_{ControllerTypes.FORWARD_POSITION.value}_controller" ) self.robot.listen_to(list(comp_action_topic.values())) self._comp_action_topic = comp_action_topic @@ -119,7 +118,7 @@ def __init__(self, config: Optional[AIRBOTMMK2Config] = None, **kwargs) -> None: def _move_by_traj(self, goal: dict): if self.config.demonstrate: # TODO: since the arms and eefs are controlled by the teleop bag - for comp in MMK2ComponentsGroup.ARMS_EEFS: + for comp in RobotComponentsGroup.ARMS_EEFS: goal.pop(comp, None) if goal: # start = time.time() @@ -149,7 +148,7 @@ def send_action(self, action, wait=False): # param = MoveServoParams(header=self.robot.get_header()) if self.config.ignore_base_action: - goal.pop(MMK2Components.BASE, None) + goal.pop(RobotComponents.BASE, None) if self.traj_mode: self._move_by_traj(goal) else: @@ -157,12 +156,12 @@ def send_action(self, action, wait=False): # param = TrackingParams() # param = MoveServoParams(header=self.robot.get_header()) # param = { - # MMK2Components.LEFT_ARM: MoveServoParams(header=self.robot.get_header()), - # MMK2Components.RIGHT_ARM: MoveServoParams(header=self.robot.get_header()), - # MMK2Components.LEFT_ARM_EEF: TrajectoryParams(), - # MMK2Components.RIGHT_ARM_EEF: TrajectoryParams(), - # MMK2Components.HEAD: ForwardPositionParams(), - # MMK2Components.SPINE: ForwardPositionParams(), + # RobotComponents.LEFT_ARM: MoveServoParams(header=self.robot.get_header()), + # RobotComponents.RIGHT_ARM: MoveServoParams(header=self.robot.get_header()), + # RobotComponents.LEFT_ARM_EEF: TrajectoryParams(), + # RobotComponents.RIGHT_ARM_EEF: TrajectoryParams(), + # RobotComponents.HEAD: ForwardPositionParams(), + # RobotComponents.SPINE: ForwardPositionParams(), # } self.robot.set_goal(goal, param) @@ -176,7 +175,7 @@ def get_low_dim_data(self): all_joints, self.joint_names[comp] ) data[f"observation/{comp.value}/joint_position"] = joint_states - if comp == MMK2Components.BASE: + if comp == RobotComponents.BASE: base_pose = robot_state.base_state.pose base_vel = robot_state.base_state.velocity data_pose = [ @@ -193,7 +192,7 @@ def get_low_dim_data(self): data[f"action/{comp.value}/velocity"] = data_vel data[f"action/{comp.value}/joint_position"] = data_vel + data_pose if self.config.demonstrate: - if comp in MMK2ComponentsGroup.ARMS: + if comp in RobotComponentsGroup.ARMS: arm_jn = JointNames().__dict__[comp.value] comp_eef = comp.value + "_eef" eef_jn = JointNames().__dict__[comp_eef] @@ -202,7 +201,7 @@ def get_low_dim_data(self): data[f"action/{comp.value}/joint_position"] = jq[:-1] # the eef joint is in arms data[f"action/{comp_eef}/joint_position"] = jq[-1:] - elif comp in MMK2ComponentsGroup.HEAD_SPINE: + elif comp in RobotComponentsGroup.HEAD_SPINE: jq = list( self.robot.get_listened(self._comp_action_topic[comp]).data ) @@ -211,7 +210,7 @@ def get_low_dim_data(self): def _capture_images(self) -> Tuple[Dict[str, bytes], Dict[str, Time]]: images = {} - img_stamps: Dict[MMK2Components, Time] = {} + img_stamps: Dict[RobotComponents, Time] = {} before_camread_t = time.perf_counter() comp_images = self.robot.get_image(self.cameras_goal) for comp, image in comp_images.items(): @@ -249,7 +248,7 @@ def low_dim_to_action(self, low_dim: dict, step: int) -> list: for comp in self.components: # action.extend(low_dim[f"action/{comp.value}/joint_position"][step]) # old version - if comp in MMK2ComponentsGroup.ARMS_EEFS: + if comp in RobotComponentsGroup.ARMS_EEFS: pos_comp = comp.value.split("_") key = f"{pos_comp[1]}/{pos_comp[0]}" else: @@ -265,7 +264,7 @@ def _action_check(self, action): len(action) == self.joint_num ), f"Invalid action {action} with length: {len(action)}" - def _action_to_goal(self, action) -> Dict[MMK2Components, JointState]: + def _action_to_goal(self, action) -> Dict[RobotComponents, JointState]: if self.config.check_dim: self._action_check(action) goal = {} diff --git a/robots/airbots/airbot_mmk/airbot_com_mmk2_bson.py b/robots/airbots/airbot_mmk/airbot_com_mmk2_bson.py index 2199a3f..52d64d9 100644 --- a/robots/airbots/airbot_mmk/airbot_com_mmk2_bson.py +++ b/robots/airbots/airbot_mmk/airbot_com_mmk2_bson.py @@ -1,13 +1,13 @@ from mmk2_types.types import ( JointNames, - MMK2Components, - MMK2ComponentsGroup, + RobotComponents, + RobotComponentsGroup, ) from mmk2_types.grpc_msgs import Time from typing import Optional, Dict import logging import numpy as np -from robots.airbots.airbot_mmk.airbot_com_mmk2 import AIRBOTMMK2Config +from robots.airbots.airbot_mmk.airbot_com_mmk2 import AIRBOTRobotConfig from robots.airbots.airbot_mmk.airbot_com_mmk2 import AIRBOTMMK2 as AIRBOTMMK2_BASE @@ -17,7 +17,7 @@ class AIRBOTMMK2(AIRBOTMMK2_BASE): - def __init__(self, config: Optional[AIRBOTMMK2Config] = None, **kwargs): + def __init__(self, config: Optional[AIRBOTRobotConfig] = None, **kwargs): self.images_ts: Dict[str, int] = {} super().__init__(config, **kwargs) @@ -68,7 +68,7 @@ def get_low_dim_data(self): all_joints = robot_state.joint_state for comp in self.components: stamp = all_joints.header.stamp - if comp != MMK2Components.BASE: + if comp != RobotComponents.BASE: # TODO: hand has no joint states names = self.joint_names[comp] if set(names) - set(all_joints.name): @@ -86,7 +86,7 @@ def get_low_dim_data(self): all_joints, names, "effort" ) # TODO: configure has-pose components - if comp in MMK2ComponentsGroup.ARMS: + if comp in RobotComponentsGroup.ARMS: poses = robot_state.robot_pose.robot_pose[comp.value] t = poses.position.x, poses.position.y, poses.position.z r = ( @@ -109,10 +109,10 @@ def get_low_dim_data(self): ) ) if self.config.demonstrate: - if comp in MMK2ComponentsGroup.ARMS: + if comp in RobotComponentsGroup.ARMS: arm_jn = JointNames[comp.name].value comp_eef = comp.value + "_eef" - eef_jn = JointNames[MMK2Components(comp_eef).name].value + eef_jn = JointNames[RobotComponents(comp_eef).name].value js = self.robot.get_listened(self._comp_action_topic[comp]) assert js is not None, "The robot should be in teleopration mode." jq = self.robot.get_joint_values_by_names(js, arm_jn + eef_jn) @@ -126,14 +126,14 @@ def get_low_dim_data(self): "action", comp_eef, js.header.stamp, jq[-1:] ) ) - elif comp == MMK2Components.BASE: + elif comp == RobotComponents.BASE: # TODO: now action and observation are the same for base data.update( self._get_joint_state( "action", comp.value, stamp, joint_pos, joint_vel, joint_eff ) ) - elif comp in MMK2ComponentsGroup.HEAD_SPINE: + elif comp in RobotComponentsGroup.HEAD_SPINE: result = self.robot.get_listened(self._comp_action_topic[comp]) assert ( result is not None @@ -192,7 +192,7 @@ def bson_dict(self): "sn": "", "firmware_version": "0.0.0", } - if comp in MMK2ComponentsGroup.ARMS: + if comp in RobotComponentsGroup.ARMS: topics[f"/observation/{comp.value}/pose"] = { "description": "", "type": "pose", diff --git a/robots/airbots/airbot_mmk/airbot_mmk2.py b/robots/airbots/airbot_mmk/airbot_mmk2.py index 3915ef5..c70fc82 100644 --- a/robots/airbots/airbot_mmk/airbot_mmk2.py +++ b/robots/airbots/airbot_mmk/airbot_mmk2.py @@ -1,10 +1,10 @@ -from mmk2_sdk.mmk2_client import AIRBOTMMK2 as AIRBOTMMK2Client +from mmk2_sdk.mmk2_client import AIRBOTMMK2 as AIRBOTRobotClient from mmk2_types.types import ( - MMK2Components, + RobotComponents, JointNames, ComponentTypes, TopicNames, - MMK2ComponentsGroup, + RobotComponentsGroup, ImageTypes, ControllerTypes, ) @@ -26,7 +26,7 @@ @dataclass -class AIRBOTMMK2Config(object): +class AIRBOTRobotConfig(object): name: str = "mmk2" domain_id: int = -1 ip: str = "192.168.11.200" @@ -35,35 +35,35 @@ class AIRBOTMMK2Config(object): cameras: Dict[str, str] = field(default_factory=lambda: {}) components: List[str] = field( default_factory=lambda: [ - MMK2Components.LEFT_ARM.value, - MMK2Components.LEFT_ARM_EEF.value, - MMK2Components.RIGHT_ARM.value, - MMK2Components.RIGHT_ARM_EEF.value, + RobotComponents.LEFT_ARM.value, + RobotComponents.LEFT_ARM_EEF.value, + RobotComponents.RIGHT_ARM.value, + RobotComponents.RIGHT_ARM_EEF.value, ] ) demonstrate: bool = False class AIRBOTMMK2(object): - def __init__(self, config: Optional[AIRBOTMMK2Config] = None, **kwargs) -> None: + def __init__(self, config: Optional[AIRBOTRobotConfig] = None, **kwargs) -> None: if config is None: - config = AIRBOTMMK2Config() + config = AIRBOTRobotConfig() self.config = replace(config, **kwargs) - self.robot = AIRBOTMMK2Client( + self.robot = AIRBOTRobotClient( self.config.ip, self.config.port, self.config.name, self.config.domain_id, ) self.joint_names = {} - self.cameras: Dict[MMK2Components, str] = {} - self.components: Dict[MMK2Components, ComponentTypes] = {} + self.cameras: Dict[RobotComponents, str] = {} + self.components: Dict[RobotComponents, ComponentTypes] = {} all_joint_names = JointNames() self.joint_num = 0 for k, v in self.config.cameras.items(): - self.cameras[MMK2Components(k)] = ImageTypes(v) + self.cameras[RobotComponents(k)] = ImageTypes(v) for comp_str in self.config.components: - comp = MMK2Components(comp_str) + comp = RobotComponents(comp_str) # TODO: get the type info from SDK self.components[comp] = ComponentTypes.UNKNOWN names = all_joint_names.__dict__[comp_str] @@ -85,7 +85,7 @@ def __init__(self, config: Optional[AIRBOTMMK2Config] = None, **kwargs) -> None: if self.config.demonstrate: comp_action_topic = { comp: TopicNames.tracking.format(component=comp.value) - for comp in MMK2ComponentsGroup.ARMS + for comp in RobotComponentsGroup.ARMS } comp_action_topic.update( { @@ -93,7 +93,7 @@ def __init__(self, config: Optional[AIRBOTMMK2Config] = None, **kwargs) -> None: component=comp.value, controller=ControllerTypes.FORWARD_POSITION.value, ) - for comp in MMK2ComponentsGroup.HEAD_SPINE + for comp in RobotComponentsGroup.HEAD_SPINE } ) self.robot.listen_to(list(comp_action_topic.values())) @@ -112,13 +112,13 @@ def __init__(self, config: Optional[AIRBOTMMK2Config] = None, **kwargs) -> None: def _move_by_traj(self, goal: dict): # goal.update( # { - # MMK2Components.HEAD: JointState(position=[0, -1.0]), - # MMK2Components.SPINE: JointState(position=[0.15]), + # RobotComponents.HEAD: JointState(position=[0, -1.0]), + # RobotComponents.SPINE: JointState(position=[0.15]), # } # ) if self.config.demonstrate: # TODO: since the arms and eefs are controlled by the teleop bag - for comp in MMK2ComponentsGroup.ARMS_EEFS: + for comp in RobotComponentsGroup.ARMS_EEFS: goal.pop(comp) if goal: self.robot.set_goal(goal, TrajectoryParams()) @@ -150,12 +150,12 @@ def send_action(self, action, wait=False): param = ForwardPositionParams() # param = MoveServoParams(header=self.robot.get_header()) # param = { - # MMK2Components.LEFT_ARM: MoveServoParams(header=self.robot.get_header()), - # MMK2Components.RIGHT_ARM: MoveServoParams(header=self.robot.get_header()), - # MMK2Components.LEFT_ARM_EEF: TrajectoryParams(), - # MMK2Components.RIGHT_ARM_EEF: TrajectoryParams(), - # MMK2Components.HEAD: ForwardPositionParams(), - # MMK2Components.SPINE: ForwardPositionParams(), + # RobotComponents.LEFT_ARM: MoveServoParams(header=self.robot.get_header()), + # RobotComponents.RIGHT_ARM: MoveServoParams(header=self.robot.get_header()), + # RobotComponents.LEFT_ARM_EEF: TrajectoryParams(), + # RobotComponents.RIGHT_ARM_EEF: TrajectoryParams(), + # RobotComponents.HEAD: ForwardPositionParams(), + # RobotComponents.SPINE: ForwardPositionParams(), # } self.robot.set_goal(goal, param) @@ -169,7 +169,7 @@ def get_low_dim_data(self): ) data[f"observation/{comp.value}/joint_position"] = joint_states if self.config.demonstrate: - if comp in MMK2ComponentsGroup.ARMS: + if comp in RobotComponentsGroup.ARMS: arm_jn = JointNames().__dict__[comp.value] comp_eef = comp.value + "_eef" eef_jn = JointNames().__dict__[comp_eef] @@ -178,7 +178,7 @@ def get_low_dim_data(self): data[f"action/{comp.value}/joint_position"] = jq[:-1] # the eef joint is in arms data[f"action/{comp_eef}/joint_position"] = jq[-1:] - elif comp in MMK2ComponentsGroup.HEAD_SPINE: + elif comp in RobotComponentsGroup.HEAD_SPINE: jq = list( self.robot.get_listened(self._comp_action_topic[comp]).data ) @@ -187,7 +187,7 @@ def get_low_dim_data(self): def _capture_images(self) -> Tuple[Dict[str, bytes], Dict[str, Time]]: images = {} - img_stamps: Dict[MMK2Components, Time] = {} + img_stamps: Dict[RobotComponents, Time] = {} before_camread_t = time.perf_counter() comp_images = self.robot.get_image(self.cameras) for comp, image in comp_images.items(): @@ -232,7 +232,7 @@ def low_dim_to_action(self, low_dim: dict, step: int) -> list: for comp in self.components: # action.extend(low_dim[f"action/{comp.value}/joint_position"][step]) # old version - if comp in MMK2ComponentsGroup.ARMS_EEFS: + if comp in RobotComponentsGroup.ARMS_EEFS: pos_comp = comp.value.split("_") key = f"{pos_comp[1]}/{pos_comp[0]}" else: @@ -248,7 +248,7 @@ def _action_check(self, action): len(action) == self.joint_num ), f"Invalid action {action} with length: {len(action)}" - def _action_to_goal(self, action) -> Dict[MMK2Components, JointState]: + def _action_to_goal(self, action) -> Dict[RobotComponents, JointState]: self._action_check(action) goal = {} j_cnt = 0 diff --git a/robots/airbots/airbot_mmk/airbot_mmk2_bson.py b/robots/airbots/airbot_mmk/airbot_mmk2_bson.py index 31e5398..27f9dcc 100644 --- a/robots/airbots/airbot_mmk/airbot_mmk2_bson.py +++ b/robots/airbots/airbot_mmk/airbot_mmk2_bson.py @@ -1,12 +1,12 @@ from mmk2_types.types import ( JointNames, - MMK2ComponentsGroup, + RobotComponentsGroup, ) from mmk2_types.grpc_msgs import Time from typing import Optional, Dict import logging import numpy as np -from robots.airbots.airbot_mmk.airbot_mmk2 import AIRBOTMMK2Config +from robots.airbots.airbot_mmk.airbot_mmk2 import AIRBOTRobotConfig from robots.airbots.airbot_mmk.airbot_mmk2 import AIRBOTMMK2 as AIRBOTMMK2_BASE @@ -16,7 +16,7 @@ class AIRBOTMMK2(AIRBOTMMK2_BASE): - def __init__(self, config: Optional[AIRBOTMMK2Config] = None, **kwargs): + def __init__(self, config: Optional[AIRBOTRobotConfig] = None, **kwargs): self.images_ts: Dict[str, int] = {} super().__init__(config, **kwargs) @@ -69,7 +69,7 @@ def get_low_dim_data(self): ) ) if self.config.demonstrate: - if comp in MMK2ComponentsGroup.ARMS: + if comp in RobotComponentsGroup.ARMS: arm_jn = JointNames().__dict__[comp.value] comp_eef = comp.value + "_eef" eef_jn = JointNames().__dict__[comp_eef] @@ -86,7 +86,7 @@ def get_low_dim_data(self): "action", comp_eef, js.header.stamp, jq[-1:] ) ) - elif comp in MMK2ComponentsGroup.HEAD_SPINE: + elif comp in RobotComponentsGroup.HEAD_SPINE: result = self.robot.get_listened(self._comp_action_topic[comp]) assert result is not None, "The AIRBOT MMK2 should be in bag teleopration sync mode." jq = list(result.data) diff --git a/robots/airbots/airbot_play/airbot_play_5.py b/robots/airbots/airbot_play/airbot_play_5.py new file mode 100644 index 0000000..77dfa55 --- /dev/null +++ b/robots/airbots/airbot_play/airbot_play_5.py @@ -0,0 +1,173 @@ +from dataclasses import dataclass, field, replace +import time +from habitats.common.robot_devices.cameras.utils import Camera +from typing import Dict, Optional, List +from airbot_py.arm import AirbotPlay + + + +@dataclass +class AIRBOTPlayConfig(object): + port: int = 50000 + start_arm_joint_position: List[float] = field( + default_factory=lambda: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + ) + start_eef_joint_position: float = 0.0 + # ONLINE_TRAJ, ONLINE_IDLE, ONLINE_SERVO, DEMONSTRATE_PREP + default_robot_mode: str = "ONLINE_IDLE" + cameras: dict = field(default_factory=lambda: {}) + display: bool = False + + def __post_init__(self): + assert self.default_robot_mode in [ + "ONLINE_TRAJ", + "ONLINE_IDLE", + "ONLINE_SERVO", + "DEMONSTRATE_PREP", + ] + + +class AIRBOTPlay(object): + def __init__(self, config: Optional[AIRBOTPlayConfig] = None, **kwargs) -> None: + if config is None: + config = AIRBOTPlayConfig() + self.config = replace(config, **kwargs) + self.cameras: Dict[str, Camera] = self.config.cameras + self.logs = {} + self.__init() + self._state_mode = "active" + self._exited = False + + def __init(self): + args = self.config + # Connect the cameras + for name in self.cameras: + self.cameras[name].connect() + # Connect the robot + self.robot = AirbotPlay( + port = args.port + ) + args.arm_type = self.robot.params["arm_type"] + time.sleep(0.3) + self.reset() + + def reset(self): + args = self.config + robot = self.robot + # set to traj mode + if args.arm_type != "replay" and robot.get_current_state() != "ONLINE_TRAJ": + assert robot.online_mode(), "online idle mode failed" + # assert robot.online_traj_mode(), "online traj mode failed" + time.sleep(0.3) + # go to start position + if args.arm_type != "replay": + if args.start_arm_joint_position is not None: + assert robot.set_target_joint_q( + args.start_arm_joint_position, blocking=True + ), "set target joint q failed" + if args.start_eef_joint_position is not None and robot.params["eef_type"] not in ["none", "E2B"]: + assert robot.set_target_end( + args.start_eef_joint_position, blocking=False + ), "set target end failed" + # enter default mode + if args.default_robot_mode == "ONLINE_TRAJ": + self.enter_traj_mode() + elif args.default_robot_mode == "ONLINE_IDLE": + self.enter_active_mode() + elif args.default_robot_mode == "ONLINE_SERVO": + self.enter_servo_mode() + else: + raise ValueError( + f"Invalid default robot mode: {args.default_robot_mode}" + ) + + self._state_mode = "active" + + def enter_traj_mode(self): + self.enter_active_mode() + if self.config.arm_type == "replay": + return + # else: + # assert self.robot.online_traj_mode(), "online traj mode failed" + time.sleep(0.5) + self._state_mode = "active" + + def enter_active_mode(self): + if self.config.arm_type == "replay": + return + else: + assert self.robot.online_mode(), "online idle mode failed" + self._state_mode = "active" + + def enter_passive_mode(self): + if self.config.arm_type == "replay": + return + else: + assert self.robot.manual_mode(), "demonstrate start mode failed" + self._state_mode = "passive" + + def enter_servo_mode(self): + self.enter_active_mode() + if self.config.arm_type == "replay": + return + # else: + # assert self.robot.online_servo_mode(), "online_servo_mode mode failed" # 没有online_servo_mode + self._state_mode = "active" + + def send_action(self, action, wait=False): + assert self._state_mode == "active", "Robot is not in active mode" + if self.config.arm_type == "replay": + return + else: + assert self.robot.set_target_joint_q( + action[:6], blocking=wait, vel=0.5, use_planning=False + ), "set target joint q failed" + if self.robot.params["eef_type"] not in ["none", "E2B"]: + # assert self.robot.set_target_end(action[6]), "set target end failed" + self.robot.set_target_end(action[6], blocking=wait) + return action + + def get_low_dim_data(self): + data = {} + data["/time"] = time.time() + pose = self.robot.get_current_pose() + data["observation/arm/joint_position"] = list(self.robot.get_current_joint_q()) + data["observation/eef/joint_position"] = [self.robot.get_current_end()] + data["observation/eef/pose"] = pose[0] + pose[1] # xyz + quat(xyzw) + return data + + def capture_observation(self): + """The returned observations do not have a batch dimension.""" + obs_act_dict = {} + # Capture images from cameras + images = {} + for name in self.cameras: + before_camread_t = time.perf_counter() + images[name] = self.cameras[name].async_read() + # images[name] = torch.from_numpy(images[name]) + obs_act_dict[f"/time/{name}"] = time.time() + self.logs[f"read_camera_{name}_dt_s"] = self.cameras[name].logs[ + "delta_timestamp_s" + ] + self.logs[f"async_read_camera_{name}_dt_s"] = ( + time.perf_counter() - before_camread_t + ) + + low_dim_data = self.get_low_dim_data() + + # Populate output dictionnaries and format to pytorch + obs_act_dict["low_dim"] = low_dim_data + for name in self.cameras: + obs_act_dict[f"observation.images.{name}"] = images[name] + return obs_act_dict + + def exit(self): + assert not self._exited, "Robot already exited" + for name in self.cameras: + self.cameras[name].disconnect() + self.robot.shutdown() + self._exited = True + print("Robot exited") + + def get_state_mode(self): + return self._state_mode From 4ad0aa20a78cf9502688ce96dc26cb9274c4cdb6 Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Thu, 10 Apr 2025 16:44:04 +0800 Subject: [PATCH 25/27] fix base control --- robots/airbots/airbot_mmk/airbot_com_mmk2.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/robots/airbots/airbot_mmk/airbot_com_mmk2.py b/robots/airbots/airbot_mmk/airbot_com_mmk2.py index 8c3c79a..4fe9e8f 100644 --- a/robots/airbots/airbot_mmk/airbot_com_mmk2.py +++ b/robots/airbots/airbot_mmk/airbot_com_mmk2.py @@ -17,6 +17,7 @@ ForwardPositionParams, Pose3D, Twist3D, + BaseControlParams, ) from typing import Optional, Dict, List, Tuple, Any from dataclasses import dataclass, replace, field @@ -152,7 +153,12 @@ def send_action(self, action, wait=False): if self.traj_mode: self._move_by_traj(goal) else: - param = ForwardPositionParams() + param = {} + for comp in goal: + if comp is RobotComponents.BASE: + param[comp] = BaseControlParams() + else: + param[comp] = ForwardPositionParams() # param = TrackingParams() # param = MoveServoParams(header=self.robot.get_header()) # param = { From 034fc3f022f648b38fa2c8ee0d5e047e7a72db72 Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Thu, 10 Apr 2025 16:45:56 +0800 Subject: [PATCH 26/27] add com mmk2 bson yaml --- .../airbots/mmk/airbot_com_mmk_bson.yaml | 47 +++++++++++++++++++ 1 file changed, 47 insertions(+) create mode 100644 configurations/basic_configs/example/robot/airbots/mmk/airbot_com_mmk_bson.yaml diff --git a/configurations/basic_configs/example/robot/airbots/mmk/airbot_com_mmk_bson.yaml b/configurations/basic_configs/example/robot/airbots/mmk/airbot_com_mmk_bson.yaml new file mode 100644 index 0000000..726eded --- /dev/null +++ b/configurations/basic_configs/example/robot/airbots/mmk/airbot_com_mmk_bson.yaml @@ -0,0 +1,47 @@ +_target_: robots.airbots.airbot_mmk.airbot_com_mmk2_bson.AIRBOTMMK2 + +ip: "192.168.11.200" + +default_action: null + +# The order is important +components: ["left_arm", "left_arm_eef", "right_arm", "right_arm_eef", "head", "spine", "base"] + +# USB cameras (only color image types) +cameras: + head_camera: + image_types: ["color"] + camera_type: "USB" + video_device: "/dev/video0" + image_width: "640" + image_height: "480" + # left_camera: + # image_types: ["color"] + # camera_type: "USB" + # video_device: "/dev/video2" + # image_width: "640" + # image_height: "480" + # right_camera: + # image_types: ["color"] + # camera_type: "USB" + # video_device: "/dev/video4" + # image_width: "640" + # image_height: "480" + +# # RealSense +# cameras: +# head_camera: +# image_types: ["color"] +# camera_type: "REALSENSE" +# serial_no: "'123456'" +# rgb_camera.color_profile: "640,480,30" +# left_camera: +# image_types: ["color"] +# camera_type: "REALSENSE" +# serial_no: "'123456'" +# rgb_camera.color_profile: "640,480,30" +# right_camera: +# image_types: ["color"] +# camera_type: "REALSENSE" +# serial_no: "'123456'" +# rgb_camera.color_profile: "640,480,30" From 86399c6c996d87c7367c54d0abf39accfb09e4fe Mon Sep 17 00:00:00 2001 From: Haizhou Ge <1352674740@qq.com> Date: Thu, 10 Apr 2025 20:08:48 +0800 Subject: [PATCH 27/27] fix base action2goal --- robots/airbots/airbot_mmk/airbot_com_mmk2.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/robots/airbots/airbot_mmk/airbot_com_mmk2.py b/robots/airbots/airbot_mmk/airbot_com_mmk2.py index 4fe9e8f..7520de9 100644 --- a/robots/airbots/airbot_mmk/airbot_com_mmk2.py +++ b/robots/airbots/airbot_mmk/airbot_com_mmk2.py @@ -277,7 +277,11 @@ def _action_to_goal(self, action) -> Dict[RobotComponents, JointState]: j_cnt = 0 for comp in self.components: end = j_cnt + len(self.joint_names[comp]) - goal[comp] = JointState(position=action[j_cnt:end]) + if comp is RobotComponents.BASE: + x, y, omega = action[j_cnt:end] + goal[comp] = Twist3D(x=x, y=y, omega=omega) + else: + goal[comp] = JointState(position=action[j_cnt:end]) j_cnt = end return goal