|
| 1 | +import logging |
| 2 | +from typing import Any |
| 3 | + |
| 4 | +import numpy as np |
| 5 | +from rcs._core import common |
| 6 | +from rcs._core.common import RobotPlatform |
| 7 | +from rcs._core.sim import SimConfig |
| 8 | +from rcs.camera.hw import HardwareCameraSet |
| 9 | +from rcs.envs.base import ControlMode |
| 10 | +from rcs.envs.creators import SimMultiEnvCreator |
| 11 | +from rcs.envs.utils import default_digit, default_sim_gripper_cfg, default_sim_robot_cfg |
| 12 | +from rcs.operator.gello import GelloConfig, GelloOperator |
| 13 | +from rcs.operator.interface import TeleopLoop |
| 14 | +from rcs.operator.quest import QuestConfig, QuestOperator |
| 15 | +from rcs_fr3.creators import RCSFR3MultiEnvCreator |
| 16 | +from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg |
| 17 | +from rcs_realsense.utils import default_realsense |
| 18 | +from simpub.sim.mj_publisher import MujocoPublisher |
| 19 | + |
| 20 | +import rcs |
| 21 | + |
| 22 | +logger = logging.getLogger(__name__) |
| 23 | + |
| 24 | + |
| 25 | +ROBOT2IP = { |
| 26 | + "right": "192.168.102.1", |
| 27 | + "left": "192.168.101.1", |
| 28 | +} |
| 29 | +ROBOT2ID = { |
| 30 | + "left": "1", |
| 31 | + "right": "0", |
| 32 | +} |
| 33 | + |
| 34 | + |
| 35 | +# ROBOT_INSTANCE = RobotPlatform.SIMULATION |
| 36 | +ROBOT_INSTANCE = RobotPlatform.HARDWARE |
| 37 | + |
| 38 | +RECORD_FPS = 30 |
| 39 | +# set camera dict to none disable cameras |
| 40 | +# CAMERA_DICT = { |
| 41 | +# "left_wrist": "230422272017", |
| 42 | +# "right_wrist": "230422271040", |
| 43 | +# "side": "243522070385", |
| 44 | +# "bird_eye": "243522070364", |
| 45 | +# } |
| 46 | +CAMERA_DICT = None |
| 47 | +MQ3_ADDR = "10.42.0.1" |
| 48 | + |
| 49 | +# DIGIT_DICT = { |
| 50 | +# "digit_right_left": "D21182", |
| 51 | +# "digit_right_right": "D21193" |
| 52 | +# } |
| 53 | +DIGIT_DICT = None |
| 54 | + |
| 55 | + |
| 56 | +DATASET_PATH = "test_data_iris_dual_arm14" |
| 57 | +INSTRUCTION = "build a tower with the blocks in front of you" |
| 58 | + |
| 59 | +robot2world = { |
| 60 | + "right": rcs.common.Pose( |
| 61 | + translation=np.array([0, 0, 0]), rpy_vector=np.array([0.89360858, -0.17453293, 0.46425758]) |
| 62 | + ), |
| 63 | + "left": rcs.common.Pose( |
| 64 | + translation=np.array([0, 0, 0]), rpy_vector=np.array([-0.89360858, -0.17453293, -0.46425758]) |
| 65 | + ), |
| 66 | +} |
| 67 | + |
| 68 | +config: QuestConfig | GelloConfig |
| 69 | +config = QuestConfig(mq3_addr=MQ3_ADDR, simulation=ROBOT_INSTANCE == RobotPlatform.SIMULATION) |
| 70 | +# config = GelloConfig( |
| 71 | +# arms={ |
| 72 | +# "right": GelloArmConfig(com_port="/dev/serial/by-id/usb-ROBOTIS_OpenRB-150_E505008B503059384C2E3120FF07332D-if00"), |
| 73 | +# "left": GelloArmConfig(com_port="/dev/serial/by-id/usb-ROBOTIS_OpenRB-150_ABA78B05503059384C2E3120FF062F26-if00"), |
| 74 | +# }, |
| 75 | +# simulation=ROBOT_INSTANCE == RobotPlatform.SIMULATION, |
| 76 | +# ) |
| 77 | + |
| 78 | + |
| 79 | +def get_env(): |
| 80 | + if ROBOT_INSTANCE == RobotPlatform.HARDWARE: |
| 81 | + |
| 82 | + cams: list[Any] = [] |
| 83 | + if CAMERA_DICT is not None: |
| 84 | + cams.append(default_realsense(CAMERA_DICT)) |
| 85 | + if DIGIT_DICT is not None: |
| 86 | + cams.append(default_digit(DIGIT_DICT)) |
| 87 | + |
| 88 | + camera_set = HardwareCameraSet(cams) if cams else None |
| 89 | + |
| 90 | + env_rel = RCSFR3MultiEnvCreator()( |
| 91 | + name2ip=ROBOT2IP, |
| 92 | + camera_set=camera_set, |
| 93 | + robot_cfg=default_fr3_hw_robot_cfg(async_control=True), |
| 94 | + control_mode=config.operator_class.control_mode[0], |
| 95 | + gripper_cfg=default_fr3_hw_gripper_cfg(async_control=True), |
| 96 | + max_relative_movement=( |
| 97 | + 0.5 if config.operator_class.control_mode[0] == ControlMode.JOINTS else (0.5, np.deg2rad(90)) |
| 98 | + ), |
| 99 | + relative_to=config.operator_class.control_mode[1], |
| 100 | + robot2world=robot2world, |
| 101 | + ) |
| 102 | + # env_rel = StorageWrapper( |
| 103 | + # env_rel, DATASET_PATH, INSTRUCTION, batch_size=32, max_rows_per_group=100, max_rows_per_file=1000 |
| 104 | + # ) |
| 105 | + operator = GelloOperator(config) if isinstance(config, GelloConfig) else QuestOperator(config) |
| 106 | + else: |
| 107 | + # FR3 |
| 108 | + rcs.scenes["duo"] = rcs.Scene( |
| 109 | + mjcf_scene="/ssd_data/juelg/rcs_modern/rcs_models/output/fr3_duo_flexible.xml", |
| 110 | + mjcf_robot=rcs.scenes["fr3_simple_pick_up"].mjcf_robot, |
| 111 | + robot_type=common.RobotType.FR3, |
| 112 | + ) |
| 113 | + |
| 114 | + robot_cfg = default_sim_robot_cfg("duo", idx="") |
| 115 | + |
| 116 | + # resolution = (256, 256) |
| 117 | + # cameras = { |
| 118 | + # cam: SimCameraConfig( |
| 119 | + # identifier=cam, |
| 120 | + # type=CameraType.fixed, |
| 121 | + # resolution_height=resolution[1], |
| 122 | + # resolution_width=resolution[0], |
| 123 | + # frame_rate=0, |
| 124 | + # ) |
| 125 | + # for cam in ["side", "wrist"] |
| 126 | + # } |
| 127 | + |
| 128 | + sim_cfg = SimConfig() |
| 129 | + sim_cfg.async_control = True |
| 130 | + env_rel = SimMultiEnvCreator()( |
| 131 | + name2id=ROBOT2ID, |
| 132 | + robot_cfg=robot_cfg, |
| 133 | + control_mode=GelloOperator.control_mode[0], |
| 134 | + gripper_cfg=default_sim_gripper_cfg(), |
| 135 | + # cameras=default_mujoco_cameraset_cfg(), |
| 136 | + max_relative_movement=0.5, |
| 137 | + relative_to=GelloOperator.control_mode[1], |
| 138 | + sim_cfg=sim_cfg, |
| 139 | + robot2world=robot2world, |
| 140 | + ) |
| 141 | + # sim = env_rel.unwrapped.envs[ROBOT2IP.keys().__iter__().__next__()].sim # type: ignore |
| 142 | + sim = env_rel.get_wrapper_attr("sim") |
| 143 | + operator = GelloOperator(config, sim) if isinstance(config, GelloConfig) else QuestOperator(config, sim) |
| 144 | + sim.open_gui() |
| 145 | + MujocoPublisher(sim.model, sim.data, MQ3_ADDR, visible_geoms_groups=list(range(1, 3))) |
| 146 | + return env_rel, operator |
| 147 | + |
| 148 | + |
| 149 | +def main(): |
| 150 | + env_rel, operator = get_env() |
| 151 | + env_rel.reset() |
| 152 | + tele = TeleopLoop(env_rel, operator) |
| 153 | + with env_rel, tele: # type: ignore |
| 154 | + tele.environment_step_loop() |
| 155 | + |
| 156 | + |
| 157 | +if __name__ == "__main__": |
| 158 | + main() |
0 commit comments