From 38b2c88571b1da005b00572ce0b813ef4a0b49f6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 9 Apr 2026 21:08:42 +0200 Subject: [PATCH 1/7] refactor: xml id to true prefix --- docs/getting_started/index.md | 4 ++-- examples/fr3/fr3_direct_control.py | 4 ++-- extensions/rcs_tacto/src/rcs_tacto/creators.py | 2 +- python/rcs/_core/sim.pyi | 6 +++--- python/rcs/envs/creators.py | 6 +++--- python/rcs/envs/sim.py | 6 +++--- python/rcs/envs/utils.py | 8 ++++---- src/pybind/rcs.cpp | 6 +++--- src/sim/SimGripper.h | 12 ++++++------ src/sim/SimRobot.h | 12 ++++++------ src/sim/SimTilburgHand.h | 12 ++++++------ src/sim/test.cpp | 4 ++-- 12 files changed, 41 insertions(+), 41 deletions(-) diff --git a/docs/getting_started/index.md b/docs/getting_started/index.md index 58a4775a..f18e9a38 100644 --- a/docs/getting_started/index.md +++ b/docs/getting_started/index.md @@ -59,13 +59,13 @@ ik = rcs.common.RL(str(urdf_path)) # Configure robot cfg = sim.SimRobotConfig() -cfg.add_id("0") +cfg.add_postfix("_0") cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) robot = rcs.sim.SimRobot(simulation, ik, cfg) # Configure gripper gripper_cfg_sim = sim.SimGripperConfig() -gripper_cfg_sim.add_id("0") +gripper_cfg_sim.add_postfix("_0") gripper = sim.SimGripper(simulation, gripper_cfg_sim) # Configure cameras diff --git a/examples/fr3/fr3_direct_control.py b/examples/fr3/fr3_direct_control.py index ae3683f3..dd485575 100644 --- a/examples/fr3/fr3_direct_control.py +++ b/examples/fr3/fr3_direct_control.py @@ -63,7 +63,7 @@ def main(): scene = rcs.scenes["fr3_empty_world"] simulation = sim.Sim(scene.mjb or scene.mjcf_scene) robot_cfg = sim.SimRobotConfig() - robot_cfg.add_id("0") + robot_cfg.add_postfix("_0") robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) ik = rcs.common.Pin( robot_cfg.kinematic_model_path, @@ -73,7 +73,7 @@ def main(): robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) gripper_cfg_sim = sim.SimGripperConfig() - gripper_cfg_sim.add_id("0") + gripper_cfg_sim.add_postfix("_0") gripper = sim.SimGripper(simulation, gripper_cfg_sim) # add camera to have a rendering gui diff --git a/extensions/rcs_tacto/src/rcs_tacto/creators.py b/extensions/rcs_tacto/src/rcs_tacto/creators.py index e78581ee..8f578c5e 100644 --- a/extensions/rcs_tacto/src/rcs_tacto/creators.py +++ b/extensions/rcs_tacto/src/rcs_tacto/creators.py @@ -83,7 +83,7 @@ def __call__( # type: ignore ] # Append the id to keep it consistent with the model - gripper_cfg.add_id("0") + gripper_cfg.add_postfix("_0") random_pos_args = {"joint_name": "yellow-box-joint"} env = SimTaskEnvCreator()( diff --git a/python/rcs/_core/sim.pyi b/python/rcs/_core/sim.pyi index c70d9209..334bd677 100644 --- a/python/rcs/_core/sim.pyi +++ b/python/rcs/_core/sim.pyi @@ -139,7 +139,7 @@ class SimGripperConfig(rcs._core.common.GripperConfig): def __copy__(self) -> SimGripperConfig: ... def __deepcopy__(self, arg0: dict) -> SimGripperConfig: ... def __init__(self) -> None: ... - def add_id(self, id: str) -> None: ... + def add_postfix(self, id: str) -> None: ... class SimGripperState(rcs._core.common.GripperState): def __init__(self) -> None: ... @@ -174,7 +174,7 @@ class SimRobotConfig(rcs._core.common.RobotConfig): def __copy__(self) -> SimRobotConfig: ... def __deepcopy__(self, arg0: dict) -> SimRobotConfig: ... def __init__(self) -> None: ... - def add_id(self, id: str) -> None: ... + def add_postfix(self, id: str) -> None: ... class SimRobotState(rcs._core.common.RobotState): def __init__(self) -> None: ... @@ -210,7 +210,7 @@ class SimTilburgHandConfig(rcs._core.common.HandConfig): min_joint_position: numpy.ndarray[tuple[typing.Literal[16]], numpy.dtype[numpy.float64]] seconds_between_callbacks: float def __init__(self) -> None: ... - def add_id(self, id: str) -> None: ... + def add_postfix(self, id: str) -> None: ... class SimTilburgHandState(rcs._core.common.HandState): def __init__(self) -> None: ... diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index 4a693bad..af189fa4 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -161,7 +161,7 @@ def __call__( # type: ignore robots: dict[str, rcs.sim.SimRobot] = {} for key, mid in name2id.items(): cfg = copy.copy(robot_cfg) - cfg.add_id(mid) + cfg.add_postfix("_" + mid) robots[key] = rcs.sim.SimRobot(sim=simulation, ik=ik, cfg=cfg) envs = {} @@ -169,7 +169,7 @@ def __call__( # type: ignore env: gym.Env = RobotEnv(robots[key], control_mode) if gripper_cfg is not None: gripper_cfg_copy = copy.copy(gripper_cfg) - gripper_cfg_copy.add_id(mid) + gripper_cfg_copy.add_postfix("_" + mid) gripper = rcs.sim.SimGripper(simulation, gripper_cfg_copy) env = GripperWrapper(env, gripper, binary=True) @@ -331,7 +331,7 @@ def __call__( # type: ignore rotation=np.array([[0.707, 0.707, 0], [-0.707, 0.707, 0], [0, 0, 1]]), # type: ignore ) robot_cfg.robot_type = rcs.common.RobotType.FR3 - robot_cfg.add_id("0") # only required for fr3 + robot_cfg.add_postfix("_0") # only required for fr3 robot_cfg.mjcf_scene_path = mjcf_path robot_cfg.kinematic_model_path = rcs.scenes["fr3_empty_world"].mjcf_robot # .urdf (in case for urdf) print( diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index 1f157d50..752c75c9 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -241,7 +241,7 @@ def env_from_xml_paths( env: gym.Env, mjmld: str, cg_kinematics_path: str, - id: str = "0", + id: str = "_0", gripper: bool = True, hand: bool = False, check_home_collision: bool = True, @@ -273,13 +273,13 @@ def env_from_xml_paths( c_env = RobotSimWrapper(c_env, simulation) if gripper: gripper_cfg = sim.SimGripperConfig() - gripper_cfg.add_id(id) + gripper_cfg.add_postfix(id) fh = sim.SimGripper(simulation, gripper_cfg) c_env = GripperWrapper(c_env, fh) c_env = GripperWrapperSim(c_env, fh) if hand: hand_cfg = default_sim_tilburg_hand_cfg() - # hand_cfg.add_id(id) + # hand_cfg.add_postfix(id) th = sim.SimTilburgHand(simulation, hand_cfg) c_env = HandWrapper(c_env, th) c_env = HandWrapperSim(c_env, th) diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py index 18b18639..dd8b6716 100644 --- a/python/rcs/envs/utils.py +++ b/python/rcs/envs/utils.py @@ -15,12 +15,12 @@ logger.setLevel(logging.INFO) -def default_sim_robot_cfg(scene: str = "fr3_empty_world", idx: str = "0") -> sim.SimRobotConfig: +def default_sim_robot_cfg(scene: str = "fr3_empty_world", idx: str = "_0") -> sim.SimRobotConfig: robot_cfg = rcs.sim.SimRobotConfig() scene_cfg = rcs.scenes[scene] robot_cfg.robot_type = scene_cfg.robot_type robot_cfg.tcp_offset = common.Pose(common.FrankaHandTCPOffset()) - robot_cfg.add_id(idx) + robot_cfg.add_postfix(idx) if (mjb := rcs.scenes[scene].mjb) is not None: robot_cfg.mjcf_scene_path = mjb else: @@ -37,9 +37,9 @@ def default_tilburg_hw_hand_cfg(file: str | PathLike | None = None) -> THConfig: return hand_cfg -def default_sim_gripper_cfg(idx: str = "0") -> sim.SimGripperConfig: +def default_sim_gripper_cfg(idx: str = "_0") -> sim.SimGripperConfig: cfg = sim.SimGripperConfig() - cfg.add_id(idx) + cfg.add_postfix(idx) return cfg diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 78726e2e..8cc5f916 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -482,7 +482,7 @@ PYBIND11_MODULE(_core, m) { [](const rcs::sim::SimRobotConfig& self, py::dict) { return rcs::sim::SimRobotConfig(self); }) - .def("add_id", &rcs::sim::SimRobotConfig::add_id, py::arg("id")); + .def("add_postfix", &rcs::sim::SimRobotConfig::add_postfix, py::arg("id")); py::class_(sim, "SimRobotState") .def(py::init<>()) @@ -528,7 +528,7 @@ PYBIND11_MODULE(_core, m) { [](const rcs::sim::SimGripperConfig& self, py::dict) { return rcs::sim::SimGripperConfig(self); }) - .def("add_id", &rcs::sim::SimGripperConfig::add_id, py::arg("id")); + .def("add_postfix", &rcs::sim::SimGripperConfig::add_postfix, py::arg("id")); py::class_( sim, "SimGripperState") .def(py::init<>()) @@ -612,7 +612,7 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("grasp_type", &rcs::sim::SimTilburgHandConfig::grasp_type) .def_readwrite("seconds_between_callbacks", &rcs::sim::SimTilburgHandConfig::seconds_between_callbacks) - .def("add_id", &rcs::sim::SimTilburgHandConfig::add_id, py::arg("id")); + .def("add_postfix", &rcs::sim::SimTilburgHandConfig::add_postfix, py::arg("id")); // SimTilburgHand py::class_>(sim, "SimTilburgHand") diff --git a/src/sim/SimGripper.h b/src/sim/SimGripper.h index 9d19df79..0835b12d 100644 --- a/src/sim/SimGripper.h +++ b/src/sim/SimGripper.h @@ -29,20 +29,20 @@ struct SimGripperConfig : common::GripperConfig { std::vector joints = {"finger_joint1", "finger_joint2"}; std::string actuator = "actuator8"; - void add_id(const std::string& id) { + void add_postfix(const std::string& id) { for (auto& s : this->collision_geoms) { - s = s + "_" + id; + s += id; } for (auto& s : this->collision_geoms_fingers) { - s = s + "_" + id; + s += id; } for (auto& s : this->ignored_collision_geoms) { - s = s + "_" + id; + s += id; } for (auto& s : this->joints) { - s = s + "_" + id; + s += id; } - this->actuator = this->actuator + "_" + id; + this->actuator += id; } }; diff --git a/src/sim/SimRobot.h b/src/sim/SimRobot.h index 206fef87..0dfac8c0 100644 --- a/src/sim/SimRobot.h +++ b/src/sim/SimRobot.h @@ -31,18 +31,18 @@ struct SimRobotConfig : common::RobotConfig { std::string base = "base"; std::string mjcf_scene_path = "assets/scenes/fr3_empty_world/scene.xml"; - void add_id(const std::string& id) { + void add_postfix(const std::string& id) { for (auto& s : this->arm_collision_geoms) { - s = s + "_" + id; + s += id; } for (auto& s : this->joints) { - s = s + "_" + id; + s += id; } for (auto& s : this->actuators) { - s = s + "_" + id; + s += id; } - this->attachment_site = this->attachment_site + "_" + id; - this->base = this->base + "_" + id; + this->attachment_site += id; + this->base += id; } }; diff --git a/src/sim/SimTilburgHand.h b/src/sim/SimTilburgHand.h index 03f19f64..42547065 100644 --- a/src/sim/SimTilburgHand.h +++ b/src/sim/SimTilburgHand.h @@ -46,21 +46,21 @@ struct SimTilburgHandConfig : common::HandConfig { common::GraspType grasp_type = common::GraspType::POWER_GRASP; double seconds_between_callbacks = 0.0167; // 60 Hz - void add_id(const std::string& id) { + void add_postfix(const std::string& id) { for (auto& s : this->collision_geoms) { - s = s + "_" + id; + s += id; } for (auto& s : this->collision_geoms_fingers) { - s = s + "_" + id; + s += id; } for (auto& s : this->ignored_collision_geoms) { - s = s + "_" + id; + s += id; } for (auto& s : this->joints) { - s = s + "_" + id; + s += id; } for (auto& s : this->actuators) { - s = s + "_" + id; + s += id; } } }; diff --git a/src/sim/test.cpp b/src/sim/test.cpp index 08c4202b..686a814b 100644 --- a/src/sim/test.cpp +++ b/src/sim/test.cpp @@ -123,7 +123,7 @@ int test_sim() { cfg.realtime = true; cfg.async_control = false; sim->set_config(cfg); - std::string id = "0"; + std::string id = "_0"; auto ik = std::make_shared(mjcf_robot, "attachment_site_" + id, false); @@ -131,7 +131,7 @@ int test_sim() { rcs::sim::SimRobotConfig fr3_config; fr3_config.tcp_offset = tcp_offset; fr3_config.seconds_between_callbacks = 0.05; // 20hz - fr3_config.add_id(id); + fr3_config.add_postfix(id); auto fr3 = rcs::sim::SimRobot(sim, ik, fr3_config); std::jthread t(rendering_loop, m, d); sim->step(1); From 5fed1cffd65d8c2eb7d5aa6fb3ed5f48c853c6e4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 10 Apr 2026 15:58:04 +0200 Subject: [PATCH 2/7] assets: add base dual arm scene --- assets/fr3/mjcf/fr3_1.xml | 154 ++++++++++++++++++ .../Panda_RealSenseD435_Camera_Mount.obj | 1 + .../Panda_RealSenseD435_Camera_Mount.stl | 1 + .../assets/bird_eye_cam_realsense_mount.stl | 1 + assets/scenes/fr3_dual_arm/assets/d435i_0.obj | 1 + assets/scenes/fr3_dual_arm/assets/d435i_1.obj | 1 + assets/scenes/fr3_dual_arm/assets/d435i_2.obj | 1 + assets/scenes/fr3_dual_arm/assets/d435i_3.obj | 1 + assets/scenes/fr3_dual_arm/assets/d435i_4.obj | 1 + assets/scenes/fr3_dual_arm/assets/d435i_5.obj | 1 + assets/scenes/fr3_dual_arm/assets/d435i_6.obj | 1 + assets/scenes/fr3_dual_arm/assets/d435i_7.obj | 1 + assets/scenes/fr3_dual_arm/assets/d435i_8.obj | 1 + .../scenes/fr3_dual_arm/assets/finger_0.obj | 1 + .../scenes/fr3_dual_arm/assets/finger_1.obj | 1 + .../scenes/fr3_dual_arm/assets/fr3_link0.stl | 1 + .../fr3_dual_arm/assets/fr3_link0_0.obj | 1 + .../fr3_dual_arm/assets/fr3_link0_1.obj | 1 + .../fr3_dual_arm/assets/fr3_link0_2.obj | 1 + .../fr3_dual_arm/assets/fr3_link0_3.obj | 1 + .../fr3_dual_arm/assets/fr3_link0_4.obj | 1 + .../fr3_dual_arm/assets/fr3_link0_5.obj | 1 + .../fr3_dual_arm/assets/fr3_link0_6.obj | 1 + .../scenes/fr3_dual_arm/assets/fr3_link1.obj | 1 + .../scenes/fr3_dual_arm/assets/fr3_link1.stl | 1 + .../scenes/fr3_dual_arm/assets/fr3_link2.obj | 1 + .../scenes/fr3_dual_arm/assets/fr3_link2.stl | 1 + .../scenes/fr3_dual_arm/assets/fr3_link3.stl | 1 + .../fr3_dual_arm/assets/fr3_link3_0.obj | 1 + .../fr3_dual_arm/assets/fr3_link3_1.obj | 1 + .../scenes/fr3_dual_arm/assets/fr3_link4.stl | 1 + .../fr3_dual_arm/assets/fr3_link4_0.obj | 1 + .../fr3_dual_arm/assets/fr3_link4_1.obj | 1 + .../scenes/fr3_dual_arm/assets/fr3_link5.stl | 1 + .../fr3_dual_arm/assets/fr3_link5_0.obj | 1 + .../fr3_dual_arm/assets/fr3_link5_1.obj | 1 + .../fr3_dual_arm/assets/fr3_link5_2.obj | 1 + .../scenes/fr3_dual_arm/assets/fr3_link6.stl | 1 + .../fr3_dual_arm/assets/fr3_link6_0.obj | 1 + .../fr3_dual_arm/assets/fr3_link6_1.obj | 1 + .../fr3_dual_arm/assets/fr3_link6_2.obj | 1 + .../fr3_dual_arm/assets/fr3_link6_3.obj | 1 + .../fr3_dual_arm/assets/fr3_link6_4.obj | 1 + .../fr3_dual_arm/assets/fr3_link6_5.obj | 1 + .../fr3_dual_arm/assets/fr3_link6_6.obj | 1 + .../fr3_dual_arm/assets/fr3_link6_7.obj | 1 + .../scenes/fr3_dual_arm/assets/fr3_link7.stl | 1 + .../fr3_dual_arm/assets/fr3_link7_0.obj | 1 + .../fr3_dual_arm/assets/fr3_link7_1.obj | 1 + .../fr3_dual_arm/assets/fr3_link7_2.obj | 1 + .../fr3_dual_arm/assets/fr3_link7_3.obj | 1 + .../fr3_dual_arm/assets/franka_hand.stl | 1 + .../fr3_dual_arm/assets/franka_hand_0.obj | 1 + .../fr3_dual_arm/assets/franka_hand_1.obj | 1 + .../fr3_dual_arm/assets/franka_hand_2.obj | 1 + .../fr3_dual_arm/assets/franka_hand_3.obj | 1 + .../fr3_dual_arm/assets/franka_hand_4.obj | 1 + assets/scenes/fr3_dual_arm/fr3_0.xml | 1 + assets/scenes/fr3_dual_arm/fr3_1.xml | 1 + assets/scenes/fr3_dual_arm/fr3_common.xml | 1 + assets/scenes/fr3_dual_arm/scene.xml | 29 ++++ python/rcs/__init__.py | 7 + 62 files changed, 249 insertions(+) create mode 100644 assets/fr3/mjcf/fr3_1.xml create mode 120000 assets/scenes/fr3_dual_arm/assets/Panda_RealSenseD435_Camera_Mount.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/Panda_RealSenseD435_Camera_Mount.stl create mode 120000 assets/scenes/fr3_dual_arm/assets/bird_eye_cam_realsense_mount.stl create mode 120000 assets/scenes/fr3_dual_arm/assets/d435i_0.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/d435i_1.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/d435i_2.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/d435i_3.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/d435i_4.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/d435i_5.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/d435i_6.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/d435i_7.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/d435i_8.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/finger_0.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/finger_1.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link0.stl create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link0_0.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link0_1.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link0_2.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link0_3.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link0_4.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link0_5.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link0_6.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link1.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link1.stl create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link2.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link2.stl create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link3.stl create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link3_0.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link3_1.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link4.stl create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link4_0.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link4_1.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link5.stl create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link5_0.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link5_1.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link5_2.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link6.stl create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link6_0.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link6_1.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link6_2.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link6_3.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link6_4.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link6_5.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link6_6.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link6_7.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link7.stl create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link7_0.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link7_1.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link7_2.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/fr3_link7_3.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/franka_hand.stl create mode 120000 assets/scenes/fr3_dual_arm/assets/franka_hand_0.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/franka_hand_1.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/franka_hand_2.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/franka_hand_3.obj create mode 120000 assets/scenes/fr3_dual_arm/assets/franka_hand_4.obj create mode 120000 assets/scenes/fr3_dual_arm/fr3_0.xml create mode 120000 assets/scenes/fr3_dual_arm/fr3_1.xml create mode 120000 assets/scenes/fr3_dual_arm/fr3_common.xml create mode 100644 assets/scenes/fr3_dual_arm/scene.xml diff --git a/assets/fr3/mjcf/fr3_1.xml b/assets/fr3/mjcf/fr3_1.xml new file mode 100644 index 00000000..a0c893f8 --- /dev/null +++ b/assets/fr3/mjcf/fr3_1.xml @@ -0,0 +1,154 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/assets/scenes/fr3_dual_arm/assets/Panda_RealSenseD435_Camera_Mount.obj b/assets/scenes/fr3_dual_arm/assets/Panda_RealSenseD435_Camera_Mount.obj new file mode 120000 index 00000000..bcb794bc --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/Panda_RealSenseD435_Camera_Mount.obj @@ -0,0 +1 @@ +../../../cameras/real_sense/obj/Panda_RealSenseD435_Camera_Mount.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/Panda_RealSenseD435_Camera_Mount.stl b/assets/scenes/fr3_dual_arm/assets/Panda_RealSenseD435_Camera_Mount.stl new file mode 120000 index 00000000..c58e37f0 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/Panda_RealSenseD435_Camera_Mount.stl @@ -0,0 +1 @@ +../../../cameras/real_sense/stl/Panda_RealSenseD435_Camera_Mount.stl \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/bird_eye_cam_realsense_mount.stl b/assets/scenes/fr3_dual_arm/assets/bird_eye_cam_realsense_mount.stl new file mode 120000 index 00000000..aade4a49 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/bird_eye_cam_realsense_mount.stl @@ -0,0 +1 @@ +../../../cameras/real_sense/stl/bird_eye_cam_realsense_mount.stl \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/d435i_0.obj b/assets/scenes/fr3_dual_arm/assets/d435i_0.obj new file mode 120000 index 00000000..764b871a --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/d435i_0.obj @@ -0,0 +1 @@ +../../../cameras/real_sense/obj/d435i_0.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/d435i_1.obj b/assets/scenes/fr3_dual_arm/assets/d435i_1.obj new file mode 120000 index 00000000..77840457 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/d435i_1.obj @@ -0,0 +1 @@ +../../../cameras/real_sense/obj/d435i_1.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/d435i_2.obj b/assets/scenes/fr3_dual_arm/assets/d435i_2.obj new file mode 120000 index 00000000..928d54f5 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/d435i_2.obj @@ -0,0 +1 @@ +../../../cameras/real_sense/obj/d435i_2.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/d435i_3.obj b/assets/scenes/fr3_dual_arm/assets/d435i_3.obj new file mode 120000 index 00000000..24897dfe --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/d435i_3.obj @@ -0,0 +1 @@ +../../../cameras/real_sense/obj/d435i_3.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/d435i_4.obj b/assets/scenes/fr3_dual_arm/assets/d435i_4.obj new file mode 120000 index 00000000..8243fe6a --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/d435i_4.obj @@ -0,0 +1 @@ +../../../cameras/real_sense/obj/d435i_4.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/d435i_5.obj b/assets/scenes/fr3_dual_arm/assets/d435i_5.obj new file mode 120000 index 00000000..24c3c4b1 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/d435i_5.obj @@ -0,0 +1 @@ +../../../cameras/real_sense/obj/d435i_5.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/d435i_6.obj b/assets/scenes/fr3_dual_arm/assets/d435i_6.obj new file mode 120000 index 00000000..93a98319 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/d435i_6.obj @@ -0,0 +1 @@ +../../../cameras/real_sense/obj/d435i_6.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/d435i_7.obj b/assets/scenes/fr3_dual_arm/assets/d435i_7.obj new file mode 120000 index 00000000..82396dd1 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/d435i_7.obj @@ -0,0 +1 @@ +../../../cameras/real_sense/obj/d435i_7.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/d435i_8.obj b/assets/scenes/fr3_dual_arm/assets/d435i_8.obj new file mode 120000 index 00000000..aa9a0467 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/d435i_8.obj @@ -0,0 +1 @@ +../../../cameras/real_sense/obj/d435i_8.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/finger_0.obj b/assets/scenes/fr3_dual_arm/assets/finger_0.obj new file mode 120000 index 00000000..d7be52c8 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/finger_0.obj @@ -0,0 +1 @@ +../../../fingers/obj/finger_0.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/finger_1.obj b/assets/scenes/fr3_dual_arm/assets/finger_1.obj new file mode 120000 index 00000000..525864ac --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/finger_1.obj @@ -0,0 +1 @@ +../../../fingers/obj/finger_1.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link0.stl b/assets/scenes/fr3_dual_arm/assets/fr3_link0.stl new file mode 120000 index 00000000..d510caf1 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link0.stl @@ -0,0 +1 @@ +../../../fr3/stl/fr3_link0.stl \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link0_0.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link0_0.obj new file mode 120000 index 00000000..43127ef4 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link0_0.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link0_0.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link0_1.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link0_1.obj new file mode 120000 index 00000000..2048aee7 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link0_1.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link0_1.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link0_2.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link0_2.obj new file mode 120000 index 00000000..b7ab5823 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link0_2.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link0_2.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link0_3.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link0_3.obj new file mode 120000 index 00000000..8aca1411 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link0_3.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link0_3.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link0_4.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link0_4.obj new file mode 120000 index 00000000..3d0f7fa5 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link0_4.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link0_4.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link0_5.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link0_5.obj new file mode 120000 index 00000000..49e606ee --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link0_5.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link0_5.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link0_6.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link0_6.obj new file mode 120000 index 00000000..608e5fb4 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link0_6.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link0_6.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link1.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link1.obj new file mode 120000 index 00000000..16c563cd --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link1.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link1.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link1.stl b/assets/scenes/fr3_dual_arm/assets/fr3_link1.stl new file mode 120000 index 00000000..17e4d804 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link1.stl @@ -0,0 +1 @@ +../../../fr3/stl/fr3_link1.stl \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link2.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link2.obj new file mode 120000 index 00000000..a84c06e7 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link2.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link2.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link2.stl b/assets/scenes/fr3_dual_arm/assets/fr3_link2.stl new file mode 120000 index 00000000..cd8051c9 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link2.stl @@ -0,0 +1 @@ +../../../fr3/stl/fr3_link2.stl \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link3.stl b/assets/scenes/fr3_dual_arm/assets/fr3_link3.stl new file mode 120000 index 00000000..64de9b4e --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link3.stl @@ -0,0 +1 @@ +../../../fr3/stl/fr3_link3.stl \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link3_0.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link3_0.obj new file mode 120000 index 00000000..576c35aa --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link3_0.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link3_0.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link3_1.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link3_1.obj new file mode 120000 index 00000000..3a653088 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link3_1.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link3_1.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link4.stl b/assets/scenes/fr3_dual_arm/assets/fr3_link4.stl new file mode 120000 index 00000000..094b8a5d --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link4.stl @@ -0,0 +1 @@ +../../../fr3/stl/fr3_link4.stl \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link4_0.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link4_0.obj new file mode 120000 index 00000000..f285626b --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link4_0.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link4_0.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link4_1.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link4_1.obj new file mode 120000 index 00000000..1f4368a9 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link4_1.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link4_1.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link5.stl b/assets/scenes/fr3_dual_arm/assets/fr3_link5.stl new file mode 120000 index 00000000..ea38dcd7 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link5.stl @@ -0,0 +1 @@ +../../../fr3/stl/fr3_link5.stl \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link5_0.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link5_0.obj new file mode 120000 index 00000000..765df164 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link5_0.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link5_0.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link5_1.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link5_1.obj new file mode 120000 index 00000000..cd880faa --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link5_1.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link5_1.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link5_2.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link5_2.obj new file mode 120000 index 00000000..b70efa30 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link5_2.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link5_2.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link6.stl b/assets/scenes/fr3_dual_arm/assets/fr3_link6.stl new file mode 120000 index 00000000..af123358 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link6.stl @@ -0,0 +1 @@ +../../../fr3/stl/fr3_link6.stl \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link6_0.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link6_0.obj new file mode 120000 index 00000000..44acdeb5 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link6_0.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link6_0.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link6_1.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link6_1.obj new file mode 120000 index 00000000..42241f68 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link6_1.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link6_1.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link6_2.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link6_2.obj new file mode 120000 index 00000000..45c4e31e --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link6_2.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link6_2.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link6_3.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link6_3.obj new file mode 120000 index 00000000..fbd51bd0 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link6_3.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link6_3.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link6_4.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link6_4.obj new file mode 120000 index 00000000..953ad9e6 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link6_4.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link6_4.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link6_5.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link6_5.obj new file mode 120000 index 00000000..b2a58f3f --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link6_5.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link6_5.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link6_6.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link6_6.obj new file mode 120000 index 00000000..32042fe4 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link6_6.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link6_6.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link6_7.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link6_7.obj new file mode 120000 index 00000000..e88427e6 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link6_7.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link6_7.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link7.stl b/assets/scenes/fr3_dual_arm/assets/fr3_link7.stl new file mode 120000 index 00000000..ca848a16 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link7.stl @@ -0,0 +1 @@ +../../../fr3/stl/fr3_link7.stl \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link7_0.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link7_0.obj new file mode 120000 index 00000000..ef995169 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link7_0.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link7_0.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link7_1.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link7_1.obj new file mode 120000 index 00000000..2f2ee5db --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link7_1.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link7_1.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link7_2.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link7_2.obj new file mode 120000 index 00000000..3e1cb093 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link7_2.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link7_2.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/fr3_link7_3.obj b/assets/scenes/fr3_dual_arm/assets/fr3_link7_3.obj new file mode 120000 index 00000000..355b5c35 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/fr3_link7_3.obj @@ -0,0 +1 @@ +../../../fr3/obj/fr3_link7_3.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/franka_hand.stl b/assets/scenes/fr3_dual_arm/assets/franka_hand.stl new file mode 120000 index 00000000..65c89d81 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/franka_hand.stl @@ -0,0 +1 @@ +../../../grippers/franka_hand/stl/franka_hand.stl \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/franka_hand_0.obj b/assets/scenes/fr3_dual_arm/assets/franka_hand_0.obj new file mode 120000 index 00000000..81a9343e --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/franka_hand_0.obj @@ -0,0 +1 @@ +../../../grippers/franka_hand/obj/franka_hand_0.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/franka_hand_1.obj b/assets/scenes/fr3_dual_arm/assets/franka_hand_1.obj new file mode 120000 index 00000000..fefd3003 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/franka_hand_1.obj @@ -0,0 +1 @@ +../../../grippers/franka_hand/obj/franka_hand_1.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/franka_hand_2.obj b/assets/scenes/fr3_dual_arm/assets/franka_hand_2.obj new file mode 120000 index 00000000..1cbe9e14 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/franka_hand_2.obj @@ -0,0 +1 @@ +../../../grippers/franka_hand/obj/franka_hand_2.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/franka_hand_3.obj b/assets/scenes/fr3_dual_arm/assets/franka_hand_3.obj new file mode 120000 index 00000000..2587a25f --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/franka_hand_3.obj @@ -0,0 +1 @@ +../../../grippers/franka_hand/obj/franka_hand_3.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/assets/franka_hand_4.obj b/assets/scenes/fr3_dual_arm/assets/franka_hand_4.obj new file mode 120000 index 00000000..13606d10 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/assets/franka_hand_4.obj @@ -0,0 +1 @@ +../../../grippers/franka_hand/obj/franka_hand_4.obj \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/fr3_0.xml b/assets/scenes/fr3_dual_arm/fr3_0.xml new file mode 120000 index 00000000..378af610 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/fr3_0.xml @@ -0,0 +1 @@ +../../fr3/mjcf/fr3_0.xml \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/fr3_1.xml b/assets/scenes/fr3_dual_arm/fr3_1.xml new file mode 120000 index 00000000..d9501f05 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/fr3_1.xml @@ -0,0 +1 @@ +../../fr3/mjcf/fr3_1.xml \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/fr3_common.xml b/assets/scenes/fr3_dual_arm/fr3_common.xml new file mode 120000 index 00000000..49d09047 --- /dev/null +++ b/assets/scenes/fr3_dual_arm/fr3_common.xml @@ -0,0 +1 @@ +../../fr3/mjcf/fr3_common.xml \ No newline at end of file diff --git a/assets/scenes/fr3_dual_arm/scene.xml b/assets/scenes/fr3_dual_arm/scene.xml new file mode 100644 index 00000000..a978c16e --- /dev/null +++ b/assets/scenes/fr3_dual_arm/scene.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py index 8c4efd3a..ad7ea6a3 100644 --- a/python/rcs/__init__.py +++ b/python/rcs/__init__.py @@ -46,6 +46,13 @@ def get_scene_urdf(scene_name: str) -> str | None: urdf=get_scene_urdf("fr3_empty_world"), robot_type=common.RobotType.FR3, ), + "fr3_dual_arm": Scene( + mjb=os.path.join(site.getsitepackages()[0], "rcs", "scenes", "fr3_dual_arm", "scene.mjb"), + mjcf_scene=os.path.join(site.getsitepackages()[0], "rcs", "scenes", "fr3_dual_arm", "scene.xml"), + mjcf_robot=os.path.join(site.getsitepackages()[0], "rcs", "scenes", "fr3_empty_world", "robot.xml"), + urdf=get_scene_urdf("fr3_empty_world"), + robot_type=common.RobotType.FR3, + ), "fr3_simple_pick_up": Scene( mjb=os.path.join(site.getsitepackages()[0], "rcs", "scenes", "fr3_simple_pick_up", "scene.mjb"), mjcf_scene=os.path.join(site.getsitepackages()[0], "rcs", "scenes", "fr3_simple_pick_up", "scene.xml"), From 833cf06fc878afbd941fe63043d5d83d74dfde69 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 10 Apr 2026 16:00:49 +0200 Subject: [PATCH 3/7] chore(rendering): improve egl loading robustness --- python/rcs/sim/egl_bootstrap.py | 43 +++++++++++++++++++-------------- 1 file changed, 25 insertions(+), 18 deletions(-) diff --git a/python/rcs/sim/egl_bootstrap.py b/python/rcs/sim/egl_bootstrap.py index 6a3d9d5e..5a016ae9 100644 --- a/python/rcs/sim/egl_bootstrap.py +++ b/python/rcs/sim/egl_bootstrap.py @@ -1,7 +1,7 @@ """ Load the EGL library, create a persistent GLContext, and register it with the C++ backend. -Globals prevent the library and context from being garbage-collected. +Globals prevent the library and context from being garbage-collected. Call `bootstrap()` to complete initialization. """ @@ -9,25 +9,32 @@ import ctypes.util import os -import mujoco.egl -import rcs._core as _cxx -from mujoco.egl import GLContext +_egl_available = False +_addr_make_current = None +_egl_display = None +_egl_context = None name = ctypes.util.find_library("EGL") -if name is None: - msg = "libEGL not found" - raise OSError(msg) - -_egl = ctypes.CDLL(name, mode=os.RTLD_LOCAL | os.RTLD_NOW) - -addr_make_current = ctypes.cast(_egl.eglMakeCurrent, ctypes.c_void_p).value - -ctx = GLContext(max_width=3840, max_height=2160) - -egl_display = mujoco.egl.EGL_DISPLAY.address -egl_context = ctx._context.address +if name is not None: + try: + import mujoco.egl + import rcs._core as _cxx + from mujoco.egl import GLContext + + _egl = ctypes.CDLL(name, mode=os.RTLD_LOCAL | os.RTLD_NOW) + _addr_make_current = ctypes.cast(_egl.eglMakeCurrent, ctypes.c_void_p).value + _ctx = GLContext(max_width=3840, max_height=2160) + _egl_display = mujoco.egl.EGL_DISPLAY.address + _egl_context = _ctx._context.address + _egl_available = True + except Exception: + pass def bootstrap(): - assert addr_make_current is not None - _cxx.common._bootstrap_egl(addr_make_current, egl_display, egl_context) + if not _egl_available: + return + import rcs._core as _cxx + + assert _addr_make_current is not None + _cxx.common._bootstrap_egl(_addr_make_current, _egl_display, _egl_context) From 6afbd26e27aef9d48cfa9d0de3ea1cb40db58e3c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 10 Apr 2026 16:10:46 +0200 Subject: [PATCH 4/7] refactor(wrappers): base environment is now platform based - base environment is no longer the robot but for hardware an empty environment and for simulation the simulator - greenlet implementation: MultiRobotWrapper steps the simulation only once for environment wraps - adapted initialization of the environments wraps - added tests to test sim greenlet implementation --- README.md | 16 +- examples/fr3/fr3_env_cartesian_control.py | 2 +- examples/fr3/fr3_env_joint_control.py | 2 +- examples/fr3/fr3_readme.py | 13 +- examples/fr3/grasp_demo.py | 18 +- examples/fr3/grasp_digit_demo.py | 13 +- examples/fr3/grasp_ompl_demo.py | 15 +- extensions/rcs_fr3/src/rcs_fr3/creators.py | 9 +- extensions/rcs_fr3/src/rcs_fr3/envs.py | 16 +- .../rcs_panda/src/rcs_panda/creators.py | 11 +- extensions/rcs_panda/src/rcs_panda/envs.py | 17 +- .../rcs_panda/panda_env_cartesian_control.py | 2 +- .../rcs_so101/src/rcs_so101/creators.py | 6 +- extensions/rcs_ur5e/src/rcs_ur5e/creators.py | 6 +- .../rcs_xarm7/src/rcs_xarm7/creators.py | 6 +- pyproject.toml | 1 + python/rcs/envs/base.py | 287 +++++++++--- python/rcs/envs/creators.py | 28 +- python/rcs/envs/sim.py | 416 +++++++----------- python/rcs/ompl/mj_ompl.py | 16 +- python/rcs/operator/interface.py | 2 +- python/rcs/rpc/client.py | 4 +- python/rcs/rpc/server.py | 9 +- python/tests/test_multi_sim.py | 113 +++++ python/tests/test_rpc.py | 2 +- python/tests/test_sim_envs.py | 80 +--- 26 files changed, 619 insertions(+), 491 deletions(-) create mode 100644 python/tests/test_multi_sim.py diff --git a/README.md b/README.md index 666f7870..eb20482e 100644 --- a/README.md +++ b/README.md @@ -41,7 +41,6 @@ Flexibly compose your Gymnasium environment to fit your exact training needs. *F ```python from time import sleep -import gymnasium as gym import numpy as np from rcs._core.sim import SimConfig from rcs.camera.sim import SimCameraSet @@ -51,7 +50,8 @@ from rcs.envs.base import ( GripperWrapper, RelativeActionSpace, RelativeTo, - RobotEnv, + RobotWrapper, + SimEnv, ) from rcs.envs.sim import GripperWrapperSim, RobotSimWrapper from rcs.envs.utils import ( @@ -82,18 +82,19 @@ if __name__ == "__main__": # base env robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) - env: gym.Env = RobotEnv(robot, ControlMode.CARTESIAN_TQuat) + env = SimEnv(simulation) + env = RobotWrapper(env, robot, ControlMode.CARTESIAN_TQuat) # gripper gripper = sim.SimGripper(simulation, gripper_cfg) env = GripperWrapper(env, gripper, binary=True) - env = RobotSimWrapper(env, simulation) - env = GripperWrapperSim(env, gripper) + env = RobotSimWrapper(env) + env = GripperWrapperSim(env) # camera camera_set = SimCameraSet(simulation, cameras, physical_units=True, render_on_demand=True) - env = CameraSetWrapper(env, camera_set, include_depth=True) + env = CameraSetWrapper(env, camera_set, include_depth=True) # type: ignore # relative actions bounded by 10cm translation and 10 degree rotation env = RelativeActionSpace(env, max_mov=(0.1, np.deg2rad(10)), relative_to=RelativeTo.LAST_STEP) @@ -104,14 +105,13 @@ if __name__ == "__main__": env.reset() # access low level robot api to get current cartesian position - print(env.unwrapped.robot.get_cartesian_position()) + print(env.get_wrapper_attr("robot").get_cartesian_position()) for _ in range(10): # move 1cm in x direction (forward) and close gripper act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "gripper": [0]} obs, reward, terminated, truncated, info = env.step(act) print(obs) - ``` > **Note:** This and other examples can be found in the [`examples/`]() folder. diff --git a/examples/fr3/fr3_env_cartesian_control.py b/examples/fr3/fr3_env_cartesian_control.py index e62bd0c0..cb37feb6 100644 --- a/examples/fr3/fr3_env_cartesian_control.py +++ b/examples/fr3/fr3_env_cartesian_control.py @@ -56,7 +56,7 @@ def main(): env_rel.reset() # access low level robot api to get current cartesian position - print(env_rel.unwrapped.robot.get_cartesian_position()) # type: ignore + print(env_rel.get_wrapper_attr("robot").get_cartesian_position()) # type: ignore for _ in range(100): for _ in range(10): diff --git a/examples/fr3/fr3_env_joint_control.py b/examples/fr3/fr3_env_joint_control.py index 3a0887d8..62c66f8a 100644 --- a/examples/fr3/fr3_env_joint_control.py +++ b/examples/fr3/fr3_env_joint_control.py @@ -56,7 +56,7 @@ def main(): input("the robot is going to move, press enter whenever you are ready") # access low level robot api to get current cartesian position - print(env_rel.unwrapped.robot.get_joint_position()) # type: ignore + print(env_rel.get_wrapper_attr("robot").get_joint_position()) # type: ignore for _ in range(100): obs, info = env_rel.reset() diff --git a/examples/fr3/fr3_readme.py b/examples/fr3/fr3_readme.py index 3f27c72f..fdf62c4e 100644 --- a/examples/fr3/fr3_readme.py +++ b/examples/fr3/fr3_readme.py @@ -1,6 +1,5 @@ from time import sleep -import gymnasium as gym import numpy as np from rcs._core.sim import SimConfig from rcs.camera.sim import SimCameraSet @@ -10,7 +9,8 @@ GripperWrapper, RelativeActionSpace, RelativeTo, - RobotEnv, + RobotWrapper, + SimEnv, ) from rcs.envs.sim import GripperWrapperSim, RobotSimWrapper from rcs.envs.utils import ( @@ -41,14 +41,15 @@ # base env robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) - env: gym.Env = RobotEnv(robot, ControlMode.CARTESIAN_TQuat) + env = SimEnv(simulation) + env = RobotWrapper(env, robot, ControlMode.CARTESIAN_TQuat) # gripper gripper = sim.SimGripper(simulation, gripper_cfg) env = GripperWrapper(env, gripper, binary=True) - env = RobotSimWrapper(env, simulation) - env = GripperWrapperSim(env, gripper) + env = RobotSimWrapper(env) + env = GripperWrapperSim(env) # camera camera_set = SimCameraSet(simulation, cameras, physical_units=True, render_on_demand=True) @@ -63,7 +64,7 @@ env.reset() # access low level robot api to get current cartesian position - print(env.unwrapped.robot.get_cartesian_position()) + print(env.get_wrapper_attr("robot").get_cartesian_position()) for _ in range(10): # move 1cm in x direction (forward) and close gripper diff --git a/examples/fr3/grasp_demo.py b/examples/fr3/grasp_demo.py index 7fb5564f..6b0bc79d 100644 --- a/examples/fr3/grasp_demo.py +++ b/examples/fr3/grasp_demo.py @@ -6,7 +6,8 @@ import mujoco import numpy as np from rcs._core.common import Pose -from rcs.envs.base import GripperWrapper, RobotEnv +from rcs._core.sim import SimRobot +from rcs.envs.base import GripperWrapper from rcs.envs.creators import FR3SimplePickUpSimEnvCreator logger = logging.getLogger(__name__) @@ -16,8 +17,8 @@ class PickUpDemo: def __init__(self, env: gym.Env): self.env = env - self.unwrapped: RobotEnv = cast(RobotEnv, self.env.unwrapped) - self.home_pose = self.unwrapped.robot.get_cartesian_position() + self._robot = cast(SimRobot, self.env.get_wrapper_attr("robot")) + self.home_pose = self._robot.get_cartesian_position() def _action(self, pose: Pose, gripper: list[float]) -> dict[str, Any]: return {"xyzrpy": pose.xyzrpy(), "gripper": [gripper]} @@ -32,7 +33,7 @@ def get_object_pose(self, geom_name) -> Pose: ) * Pose( rpy_vector=np.array([0, 0, np.pi]), translation=[0, 0, 0] # type: ignore ) - return self.unwrapped.robot.to_pose_in_robot_coordinates(obj_pose_world_coordinates) + return self._robot.to_pose_in_robot_coordinates(obj_pose_world_coordinates) def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: int) -> list[Pose]: waypoints = [] @@ -45,12 +46,13 @@ def step(self, action: dict) -> dict: return self.env.step(action)[0] def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 20) -> list[Pose]: - end_eff_pose = self.unwrapped.robot.get_cartesian_position() + end_eff_pose = self._robot.get_cartesian_position() goal_pose = self.get_object_pose(geom_name=geom_name) goal_pose *= Pose(translation=np.array([0, 0, delta_up]), quaternion=np.array([1, 0, 0, 0])) # type: ignore return self.generate_waypoints(end_eff_pose, goal_pose, num_waypoints=num_waypoints) def execute_motion(self, waypoints: list[Pose], gripper: list[float] = GripperWrapper.BINARY_GRIPPER_OPEN) -> dict: + obs = {} for i in range(len(waypoints)): obs = self.step(self._action(waypoints[i], gripper)) return obs @@ -65,13 +67,13 @@ def grasp(self, geom_name: str): self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_OPEN) for _ in range(4): - self.step(self._action(self.unwrapped.robot.get_cartesian_position(), GripperWrapper.BINARY_GRIPPER_CLOSED)) + self.step(self._action(self._robot.get_cartesian_position(), GripperWrapper.BINARY_GRIPPER_CLOSED)) waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=60) self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_CLOSED) def move_home(self): - end_eff_pose = self.unwrapped.robot.get_cartesian_position() + end_eff_pose = self._robot.get_cartesian_position() waypoints = self.generate_waypoints(end_eff_pose, self.home_pose, num_waypoints=60) self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_CLOSED) @@ -90,7 +92,7 @@ def main(): sleep(3) for _ in range(100): env.reset() - print(env.unwrapped.robot.get_cartesian_position().translation()) # type: ignore + print(env.get_wrapper_attr("robot").get_cartesian_position().translation()) # type: ignore controller = PickUpDemo(env) controller.pickup("box_geom") diff --git a/examples/fr3/grasp_digit_demo.py b/examples/fr3/grasp_digit_demo.py index 1f746b66..c90fccad 100644 --- a/examples/fr3/grasp_digit_demo.py +++ b/examples/fr3/grasp_digit_demo.py @@ -5,7 +5,8 @@ import mujoco import numpy as np from rcs._core.common import Pose -from rcs.envs.base import GripperWrapper, RobotEnv +from rcs._core.sim import SimRobot +from rcs.envs.base import GripperWrapper from rcs_tacto.creators import FR3TactoSimplePickUpSimEnvCreator from tqdm import tqdm @@ -16,8 +17,8 @@ class PickUpDemo: def __init__(self, env: gym.Env): self.env = env - self.unwrapped: RobotEnv = cast(RobotEnv, self.env.unwrapped) - self.home_pose = self.unwrapped.robot.get_cartesian_position() + self._robot = cast(SimRobot, self.env.get_wrapper_attr("robot")) + self.home_pose = self._robot.get_cartesian_position() def _action(self, pose: Pose, gripper: list[float]) -> dict[str, Any]: return {"xyzrpy": pose.xyzrpy(), "gripper": gripper} @@ -30,7 +31,7 @@ def get_object_pose(self, geom_name) -> Pose: obj_pose_world_coordinates = Pose( translation=data.geom_xpos[geom_id], rotation=data.geom_xmat[geom_id].reshape(3, 3) ) - return self.unwrapped.robot.to_pose_in_robot_coordinates(obj_pose_world_coordinates) + return self._robot.to_pose_in_robot_coordinates(obj_pose_world_coordinates) def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: int) -> list[Pose]: waypoints = [] @@ -43,7 +44,7 @@ def step(self, action: dict) -> dict: return self.env.step(action)[0] def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 200) -> list[Pose]: - end_eff_pose = self.unwrapped.robot.get_cartesian_position() + end_eff_pose = self._robot.get_cartesian_position() goal_pose = self.get_object_pose(geom_name=geom_name) goal_pose *= Pose(translation=np.array([0, 0, delta_up]), quaternion=np.array([1, 0, 0, 0])) # type: ignore return self.generate_waypoints(end_eff_pose, goal_pose, num_waypoints=num_waypoints) @@ -68,7 +69,7 @@ def grasp(self, geom_name: str): self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_CLOSED) def move_home(self): - end_eff_pose = self.unwrapped.robot.get_cartesian_position() + end_eff_pose = self._robot.get_cartesian_position() waypoints = self.generate_waypoints(end_eff_pose, self.home_pose, num_waypoints=10) self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_CLOSED) diff --git a/examples/fr3/grasp_ompl_demo.py b/examples/fr3/grasp_ompl_demo.py index 0a05fd5d..3fdaa384 100644 --- a/examples/fr3/grasp_ompl_demo.py +++ b/examples/fr3/grasp_ompl_demo.py @@ -6,7 +6,8 @@ import mujoco import numpy as np from rcs._core.common import Pose -from rcs.envs.base import ControlMode, GripperWrapper, RobotEnv +from rcs._core.sim import SimRobot +from rcs.envs.base import ControlMode, GripperWrapper from rcs.envs.creators import FR3SimplePickUpSimEnvCreator from rcs.ompl.mj_ompl import MjOMPL @@ -38,9 +39,9 @@ class OmplTrajectoryDemo: def __init__(self, env: gym.Env, planner: MjOMPL): self.env = env - self.unwrapped: RobotEnv = cast(RobotEnv, self.env.unwrapped) - self.home_pose: Pose = self.unwrapped.robot.get_cartesian_position() - self.home_qpos: np.ndarray = self.unwrapped.robot.get_joint_position() + self._robot = cast(SimRobot, self.env.get_wrapper_attr("robot")) + self.home_pose: Pose = self._robot.get_cartesian_position() + self.home_qpos: np.ndarray = self._robot.get_joint_position() self.sol_path = None self.planner = planner @@ -60,7 +61,7 @@ def get_object_pose(self, geom_name) -> Pose: ) * Pose( rpy_vector=np.array([0, 0, np.pi]), translation=[0, 0, 0] # type: ignore ) - return self.unwrapped.robot.to_pose_in_robot_coordinates(obj_pose_world_coordinates) + return self._robot.to_pose_in_robot_coordinates(obj_pose_world_coordinates) def plan_path_to_object(self, obj_name: str, delta_up): self.move_home() @@ -83,7 +84,7 @@ def approach_and_grasp(self, obj_name: str, delta_up: float = 0.2): obj_pose_grasp = obj_pose_og * Pose(translation=np.array([0, 0, delta_up]), quaternion=np.array([1, 0, 0, 0])) # type: ignore waypoints = self.generate_waypoints( - start_pose=self.unwrapped.robot.get_cartesian_position(), end_pose=obj_pose_grasp, num_waypoints=5 + start_pose=self._robot.get_cartesian_position(), end_pose=obj_pose_grasp, num_waypoints=5 ) for waypoint in waypoints: self.step(self._jaction(waypoint, GripperWrapper.BINARY_GRIPPER_OPEN)) # type: ignore @@ -108,7 +109,7 @@ def execute_motion(self, waypoints: list[Pose], gripper: list[float] = GripperWr return obs def move_home(self): - end_eff_pose = self.unwrapped.robot.get_cartesian_position() + end_eff_pose = self._robot.get_cartesian_position() waypoints = self.generate_waypoints(end_eff_pose, self.home_pose, num_waypoints=15) self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_CLOSED) diff --git a/extensions/rcs_fr3/src/rcs_fr3/creators.py b/extensions/rcs_fr3/src/rcs_fr3/creators.py index 488dbffa..ed970a78 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/creators.py +++ b/extensions/rcs_fr3/src/rcs_fr3/creators.py @@ -13,10 +13,11 @@ ControlMode, GripperWrapper, HandWrapper, + HardwareEnv, MultiRobotWrapper, RelativeActionSpace, RelativeTo, - RobotEnv, + RobotWrapper, ) from rcs.envs.creators import RCSHardwareEnvCreator from rcs.hand.tilburg_hand import TilburgHand @@ -91,7 +92,8 @@ def __call__( # type: ignore robot = hw.Franka(ip, ik) robot.set_config(robot_cfg) - env: gym.Env = RobotEnv(robot, ControlMode.JOINTS if collision_guard is not None else control_mode) + env = HardwareEnv() + env = RobotWrapper(env, robot, ControlMode.JOINTS if collision_guard is not None else control_mode) env = FR3HW(env) if isinstance(gripper_cfg, hw.FHConfig): @@ -154,7 +156,8 @@ def __call__( # type: ignore envs = {} for key, ip in name2ip.items(): - env: gym.Env = RobotEnv(robots[key], control_mode) + env = HardwareEnv() + env = RobotWrapper(env, robots[key], control_mode) env = FR3HW(env) if gripper_cfg is not None: gripper = hw.FrankaHand(ip, gripper_cfg) diff --git a/extensions/rcs_fr3/src/rcs_fr3/envs.py b/extensions/rcs_fr3/src/rcs_fr3/envs.py index 45413a87..8b780691 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/envs.py +++ b/extensions/rcs_fr3/src/rcs_fr3/envs.py @@ -2,7 +2,7 @@ from typing import Any, SupportsFloat, cast import gymnasium as gym -from rcs.envs.base import RobotEnv +from rcs._core.common import RobotPlatform from rcs_fr3._core import hw _logger = logging.getLogger(__name__) @@ -11,9 +11,9 @@ class FR3HW(gym.Wrapper): def __init__(self, env): super().__init__(env) - self.unwrapped: RobotEnv - assert isinstance(self.unwrapped.robot, hw.Franka), "Robot must be a hw.Franka instance." - self.hw_robot = cast(hw.Franka, self.unwrapped.robot) + assert self.env.get_wrapper_attr("PLATFORM") == RobotPlatform.HARDWARE, "Base environment must be hardware." + assert isinstance(self.get_wrapper_attr("robot"), hw.Franka), "Robot must be a hw.Franka instance." + self.hw_robot = cast(hw.Franka, self.get_wrapper_attr("robot")) self._robot_state_keys: list[str] | None = None def step(self, action: Any) -> tuple[dict[str, Any], SupportsFloat, bool, bool, dict]: @@ -24,14 +24,12 @@ def step(self, action: Any) -> tuple[dict[str, Any], SupportsFloat, bool, bool, except hw.exceptions.FrankaControlException as e: _logger.error("FrankaControlException: %s", e) self.hw_robot.automatic_error_recovery() - # TODO: this does not work if some wrappers are in between - # FR3HW and RobotEnv return self.get_obs(), 0, False, True, {} def get_obs(self, obs: dict | None = None) -> dict[str, Any]: if obs is None: - obs = dict(self.unwrapped.get_obs()) - robot_state = cast(hw.FrankaState, self.unwrapped.robot.get_state()) + obs = dict(self.get_wrapper_attr("get_robot_obs")()) + robot_state = cast(hw.FrankaState, self.hw_robot.get_state()) obs["robot_state"] = self._rs2dict(robot_state.robot_state) return obs @@ -44,7 +42,7 @@ def _rs2dict(self, state: hw.RobotState): return {key: getattr(state, key) for key in self._robot_state_keys} def reset( - self, seed: int | None = None, options: dict[str, Any] | None = None + self, *, seed: int | None = None, options: dict[str, Any] | None = None ) -> tuple[dict[str, Any], dict[str, Any]]: return super().reset(seed=seed, options=options) diff --git a/extensions/rcs_panda/src/rcs_panda/creators.py b/extensions/rcs_panda/src/rcs_panda/creators.py index a605cb1d..37bcfe98 100644 --- a/extensions/rcs_panda/src/rcs_panda/creators.py +++ b/extensions/rcs_panda/src/rcs_panda/creators.py @@ -9,10 +9,11 @@ ControlMode, GripperWrapper, HandWrapper, + HardwareEnv, MultiRobotWrapper, RelativeActionSpace, RelativeTo, - RobotEnv, + RobotWrapper, ) from rcs.envs.creators import RCSHardwareEnvCreator from rcs.hand.tilburg_hand import TilburgHand @@ -68,7 +69,9 @@ def __call__( # type: ignore robot = hw.Franka(ip, ik) robot.set_config(robot_cfg) - env: gym.Env = RobotEnv( + env = HardwareEnv() + env = RobotWrapper( + env, robot, ControlMode.JOINTS if collision_guard is not None else control_mode, home_on_reset=True, @@ -109,6 +112,7 @@ def __call__( # type: ignore class RCSPandaMultiEnvCreator(RCSHardwareEnvCreator): def __call__( # type: ignore + self, ips: list[str], control_mode: ControlMode, robot_cfg: hw.PandaConfig, @@ -132,7 +136,8 @@ def __call__( # type: ignore envs = {} for ip in ips: - env: gym.Env = RobotEnv(robots[ip], control_mode) + env = HardwareEnv() + env = RobotWrapper(env, robots[ip], control_mode) env = PandaHW(env) if gripper_cfg is not None: gripper = hw.FrankaHand(ip, gripper_cfg) diff --git a/extensions/rcs_panda/src/rcs_panda/envs.py b/extensions/rcs_panda/src/rcs_panda/envs.py index 58a80212..34e8d1b1 100644 --- a/extensions/rcs_panda/src/rcs_panda/envs.py +++ b/extensions/rcs_panda/src/rcs_panda/envs.py @@ -2,7 +2,7 @@ from typing import Any, SupportsFloat, cast import gymnasium as gym -from rcs.envs.base import RobotEnv +from rcs._core.common import RobotPlatform from rcs_panda._core import hw _logger = logging.getLogger(__name__) @@ -11,9 +11,9 @@ class PandaHW(gym.Wrapper): def __init__(self, env): super().__init__(env) - self.unwrapped: RobotEnv - assert isinstance(self.unwrapped.robot, hw.Franka), "Robot must be a hw.Franka instance." - self.hw_robot = cast(hw.Franka, self.unwrapped.robot) + assert self.env.get_wrapper_attr("PLATFORM") == RobotPlatform.HARDWARE, "Base environment must be hardware." + assert isinstance(self.get_wrapper_attr("robot"), hw.Franka), "Robot must be a hw.Franka instance." + self.hw_robot = cast(hw.Franka, self.get_wrapper_attr("robot")) self._robot_state_keys: list[str] | None = None def step(self, action: Any) -> tuple[dict[str, Any], SupportsFloat, bool, bool, dict]: @@ -24,14 +24,12 @@ def step(self, action: Any) -> tuple[dict[str, Any], SupportsFloat, bool, bool, except hw.exceptions.FrankaControlException as e: _logger.error("FrankaControlException: %s", e) self.hw_robot.automatic_error_recovery() - # TODO: this does not work if some wrappers are in between - # PandaHW and RobotEnv return self.get_obs(), 0, False, True, {} def get_obs(self, obs: dict | None = None) -> dict[str, Any]: if obs is None: - obs = dict(self.unwrapped.get_obs()) - robot_state = cast(hw.FrankaState, self.unwrapped.robot.get_state()) + obs = dict(self.get_wrapper_attr("get_robot_obs")()) + robot_state = cast(hw.FrankaState, self.hw_robot.get_state()) obs["robot_state"] = self._rs2dict(robot_state.robot_state) return obs @@ -40,10 +38,11 @@ def _rs2dict(self, state: hw.RobotState): self._robot_state_keys = [ attr for attr in dir(state) if not attr.startswith("__") and not callable(getattr(state, attr)) ] + self._robot_state_keys.remove("robot_mode") return {key: getattr(state, key) for key in self._robot_state_keys} def reset( - self, seed: int | None = None, options: dict[str, Any] | None = None + self, *, seed: int | None = None, options: dict[str, Any] | None = None ) -> tuple[dict[str, Any], dict[str, Any]]: return super().reset(seed=seed, options=options) diff --git a/extensions/rcs_panda/src/rcs_panda/panda_env_cartesian_control.py b/extensions/rcs_panda/src/rcs_panda/panda_env_cartesian_control.py index 418bbf2f..21773aec 100644 --- a/extensions/rcs_panda/src/rcs_panda/panda_env_cartesian_control.py +++ b/extensions/rcs_panda/src/rcs_panda/panda_env_cartesian_control.py @@ -24,7 +24,7 @@ def main(): input("moving") env_rel.reset() - print(env_rel.unwrapped.robot.get_cartesian_position()) # type: ignore + print(env_rel.get_wrapper_attr("robot").get_cartesian_position()) # type: ignore for _ in range(100): for _ in range(10): diff --git a/extensions/rcs_so101/src/rcs_so101/creators.py b/extensions/rcs_so101/src/rcs_so101/creators.py index d891361d..52054d36 100644 --- a/extensions/rcs_so101/src/rcs_so101/creators.py +++ b/extensions/rcs_so101/src/rcs_so101/creators.py @@ -6,9 +6,10 @@ CameraSetWrapper, ControlMode, GripperWrapper, + HardwareEnv, RelativeActionSpace, RelativeTo, - RobotEnv, + RobotWrapper, ) from rcs.envs.creators import RCSHardwareEnvCreator from rcs_so101 import SO101IK @@ -33,7 +34,8 @@ def __call__( # type: ignore urdf=robot_cfg.kinematic_model_path.endswith(".urdf"), ) robot = SO101(robot_cfg=robot_cfg, ik=ik) - env: gym.Env = RobotEnv(robot, control_mode, home_on_reset=True) + env = HardwareEnv() + env = RobotWrapper(env, robot, control_mode, home_on_reset=True) gripper = SO101Gripper(robot._hf_robot, robot) env = GripperWrapper(env, gripper, binary=False) diff --git a/extensions/rcs_ur5e/src/rcs_ur5e/creators.py b/extensions/rcs_ur5e/src/rcs_ur5e/creators.py index f56353d8..6762b8d2 100644 --- a/extensions/rcs_ur5e/src/rcs_ur5e/creators.py +++ b/extensions/rcs_ur5e/src/rcs_ur5e/creators.py @@ -6,9 +6,10 @@ CameraSetWrapper, ControlMode, GripperWrapper, + HardwareEnv, RelativeActionSpace, RelativeTo, - RobotEnv, + RobotWrapper, ) from rcs.envs.creators import RCSHardwareEnvCreator from rcs_ur5e.hw import RobotiQGripper, UR5e, UR5eConfig @@ -36,7 +37,8 @@ def __call__( # type: ignore ) robot = UR5e(ip, ik) robot.set_config(robot_cfg) - env: gym.Env = RobotEnv(robot, control_mode, home_on_reset=True) + env = HardwareEnv() + env = RobotWrapper(env, robot, control_mode, home_on_reset=True) gripper = RobotiQGripper(ip) env = GripperWrapper(env, gripper, binary=True) diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py index 1a63d355..efe4e5ad 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py +++ b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py @@ -8,9 +8,10 @@ CameraSetWrapper, ControlMode, HandWrapper, + HardwareEnv, RelativeActionSpace, RelativeTo, - RobotEnv, + RobotWrapper, ) from rcs.envs.creators import RCSHardwareEnvCreator from rcs.hand.tilburg_hand import THConfig, TilburgHand @@ -34,7 +35,8 @@ def __call__( # type: ignore if isinstance(calibration_dir, str): calibration_dir = Path(calibration_dir) robot = XArm7(ip=ip) - env: gym.Env = RobotEnv(robot, control_mode, home_on_reset=True) + env = HardwareEnv() + env = RobotWrapper(env, robot, control_mode, home_on_reset=True) if camera_set is not None: camera_set.start() diff --git a/pyproject.toml b/pyproject.toml index f9f56376..d57bd7ca 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -39,6 +39,7 @@ dependencies = [ "simplejpeg", "mujoco==3.2.6", "pin==3.7.0", + "greenlet", ] readme = "README.md" maintainers = [{ name = "Tobias Jülg", email = "tobias.juelg@utn.de" }] diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index 9ae52a89..890934bb 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -5,9 +5,11 @@ from enum import Enum, auto from typing import Annotated, Any, ClassVar, Literal, TypeAlias, cast +from greenlet import getcurrent, greenlet import gymnasium as gym import numpy as np -from rcs._core.common import Hand +from rcs._core.common import Hand, RobotPlatform +from rcs._core.sim import SimRobot from rcs.camera.interface import BaseCameraSet from rcs.envs.space_utils import ( ActObsInfoWrapper, @@ -22,6 +24,8 @@ ) from rcs import common +from rcs import sim as simulation +from rcs.utils import SimpleFrameRate _logger = logging.getLogger(__name__) @@ -196,9 +200,69 @@ def get_home_position(robot: common.Robot) -> np.ndarray: """Returns the home position of the robot.""" return common.robots_meta_config(robot.get_config().robot_type).q_home +class BaseEnv(gym.Env): + PLATFORM: RobotPlatform -class RobotEnv(gym.Env): - """Joint Gym Environment for a single robot arm. + def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, bool, dict]: + return {}, 0, False, False, {} + + def reset( + self, *, seed: int | None = None, options: dict[str, Any] | None = None + ) -> tuple[dict[str, Any], dict[str, Any]]: + super().reset(seed=seed, options=options) + return {}, {} + + + +class HardwareEnv(BaseEnv): + PLATFORM = RobotPlatform.HARDWARE + + +class SimEnv(BaseEnv): + PLATFORM = RobotPlatform.SIMULATION + + def __init__(self, sim: simulation.Sim) -> None: + self.sim = sim + cfg = self.sim.get_config() + self.frame_rate = SimpleFrameRate(1 / cfg.frequency, "MoJoCo Simulation Loop") + self.main_greenlet: greenlet | None = None + + def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, bool, dict]: + if self.main_greenlet is not None: + self.main_greenlet.switch() + else: + self.step_sim() + return super().step(action) + + def step_sim(self): + cfg = self.sim.get_config() + if cfg.async_control: + self.sim.step(round(1 / cfg.frequency / self.sim.model.opt.timestep)) + if cfg.realtime: + self.frame_rate.frame_rate = 1 / cfg.frequency + self.frame_rate() + else: + self.sim.step_until_convergence() + + def reset_sim(self): + self.sim.reset() + self.sim.step(1) + + + def reset( + self, *, seed: int | None = None, options: dict[str, Any] | None = None + ) -> tuple[dict[str, Any], dict[str, Any]]: + if self.main_greenlet is not None: + self.main_greenlet.switch() + else: + self.reset_sim() + return super().reset(seed=seed, options=options) + + + + +class RobotWrapper(ActObsInfoWrapper): + """Gym Wrapper for a single robot arm. Top view of on the robot. Robot faces into x direction. z direction faces upwards. (Right handed coordinate axis) @@ -208,7 +272,8 @@ class RobotEnv(gym.Env): y """ - def __init__(self, robot: common.Robot, control_mode: ControlMode, home_on_reset: bool = False): + def __init__(self, env, robot: common.Robot, control_mode: ControlMode, home_on_reset: bool = False): + super().__init__(env) self.robot = robot self._control_mode_overrides = [control_mode] self.action_space: gym.spaces.Dict @@ -247,7 +312,7 @@ def override_control_mode(self, control_mode: ControlMode): Use this in a wrapper that wants to modify the control mode""" self._control_mode_overrides.append(control_mode) - def get_obs(self) -> ArmObsType: + def get_robot_obs(self) -> ArmObsType: return ArmObsType( tquat=np.concatenate( [self.robot.get_cartesian_position().translation(), self.robot.get_cartesian_position().rotation_q()] # type: ignore @@ -256,73 +321,107 @@ def get_obs(self) -> ArmObsType: xyzrpy=self.robot.get_cartesian_position().xyzrpy(), ) - def step(self, action: CartOrJointContType) -> tuple[ArmObsType, float, bool, bool, dict]: - action_dict = cast(dict, action) + + def action(self, action: dict[str, Any]) -> dict[str, Any]: if ( self.get_base_control_mode() == ControlMode.CARTESIAN_TQuat - and self.tquat_key not in action_dict + and self.tquat_key not in action or self.get_base_control_mode() == ControlMode.CARTESIAN_TRPY - and self.trpy_key not in action_dict + and self.trpy_key not in action or self.get_base_control_mode() == ControlMode.JOINTS - and self.joints_key not in action_dict + and self.joints_key not in action ): msg = "Given type is not matching control mode!" raise RuntimeError(msg) + self.prev_action = copy.deepcopy(action) if self.get_base_control_mode() == ControlMode.JOINTS and ( self.prev_action is None - or not np.allclose(action_dict[self.joints_key], self.prev_action[self.joints_key], atol=1e-03, rtol=0) + or not np.allclose(action[self.joints_key], self.prev_action[self.joints_key], atol=1e-03, rtol=0) ): - self.robot.set_joint_position(action_dict[self.joints_key]) + self.robot.set_joint_position(action[self.joints_key]) + action.pop(self.joints_key) elif self.get_base_control_mode() == ControlMode.CARTESIAN_TRPY and ( self.prev_action is None - or not np.allclose(action_dict[self.trpy_key], self.prev_action[self.trpy_key], atol=1e-03, rtol=0) + or not np.allclose(action[self.trpy_key], self.prev_action[self.trpy_key], atol=1e-03, rtol=0) ): self.robot.set_cartesian_position( - common.Pose(translation=action_dict[self.trpy_key][:3], rpy_vector=action_dict[self.trpy_key][3:]) + common.Pose(translation=action[self.trpy_key][:3], rpy_vector=action[self.trpy_key][3:]) ) + action.pop(self.trpy_key) elif self.get_base_control_mode() == ControlMode.CARTESIAN_TQuat and ( self.prev_action is None - or not np.allclose(action_dict[self.tquat_key], self.prev_action[self.tquat_key], atol=1e-03, rtol=0) + or not np.allclose(action[self.tquat_key], self.prev_action[self.tquat_key], atol=1e-03, rtol=0) ): self.robot.set_cartesian_position( - common.Pose(translation=action_dict[self.tquat_key][:3], quaternion=action_dict[self.tquat_key][3:]) + common.Pose(translation=action[self.tquat_key][:3], quaternion=action[self.tquat_key][3:]) ) - self.prev_action = copy.deepcopy(action_dict) - return self.get_obs(), 0, False, False, {} + action.pop(self.tquat_key) + return action + + def observation(self, observation: dict, info: dict[str, Any]) -> tuple[dict[str, Any], dict[str, Any]]: + observation.update(self.get_robot_obs()) + # if self.env.get_wrapper_attr("PLATFORM") == RobotPlatform.SIMULATION: + # sim_robot = cast(SimRobot, self.robot) + # state = sim_robot.get_state() + # info["collision"] = state.collision + # info["ik_success"] = state.ik_success + # info["is_sim_converged"] = self.env.get_wrapper_attr("sim").is_converged() + return observation, info + def reset( - self, seed: int | None = None, options: dict[str, Any] | None = None - ) -> tuple[ArmObsType, dict[str, Any]]: - if seed is not None: - msg = "seeding not implemented yet. Ignoring seed." - # raise NotImplementedError(msg) - _logger.error(msg) - if options is not None: - msg = "options not implemented yet. Ignoring options." - # raise NotImplementedError(msg) - _logger.error(msg) + self, *, seed: int | None = None, options: dict[str, Any] | None = None + ) -> tuple[dict[str, Any], dict[str, Any]]: self.robot.reset() if self.home_on_reset: self.robot.move_home() - return self.get_obs(), {} + return super().reset(seed=seed, options=options) def close(self): self.robot.close() + super().close() class MultiRobotWrapper(gym.Env): - """Wraps a dictionary of environments to allow for multi robot control.""" + """Wraps a dictionary of single robot environments to allow for multi robot control. + + Env1 Env2 Env3 + | | | + ---------------- + MultiRobotWrapper + + All envs are stepped sequentially. Supports offset of robot bases by the `robot2world` parameter. + """ + + PLATFORM: RobotPlatform | None = None def __init__( self, envs: dict[str, gym.Env] | dict[str, gym.Wrapper], robot2world: dict[str, common.Pose] | None = None ): self.envs = envs - self.unwrapped_multi = cast(dict[str, RobotEnv], {key: env.unwrapped for key, env in envs.items()}) if robot2world is None: self.robot2world = {} else: self.robot2world = robot2world + self.lead_env: gym.Env | None = None + + # make sure all envs are the same type (sim/real) + for env in self.envs: + if self.PLATFORM is None: + self.PLATFORM = self.envs[env].get_wrapper_attr("PLATFORM") + self.lead_env = self.envs[env].unwrapped + else: + assert self.envs[env].get_wrapper_attr("PLATFORM") == self.PLATFORM, "all envs must have the same platform!" + self._runs_in_sim = self.PLATFORM == RobotPlatform.SIMULATION + if self._runs_in_sim: + self._inject_main_greenlet() + + def _inject_main_greenlet(self): + main_gr = getcurrent() + for env_item in self.envs.values(): + assert isinstance(env_item.unwrapped, SimEnv), "something is wrong with the env, the base should be type SimEnv" + env_item.unwrapped.main_greenlet = main_gr def _translate_pose(self, key, dic, to_world=True): r2w = self.robot2world.get(key, common.Pose()) @@ -338,6 +437,28 @@ def _translate_pose(self, key, dic, to_world=True): return dic def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, bool, dict[str, Any]]: + step_greenlets = {} + if self._runs_in_sim: + # SIM path: 1. DOWN: Set actions for all robots + for key, env in self.envs.items(): + + def make_step_gr(env_to_step): + return greenlet(env_to_step.step) + + gr = make_step_gr(env) + step_greenlets[key] = gr + + # Translate action + act = self._translate_pose(key, action[key], to_world=False) + + # Switch to robot greenlet. It will run until RobotSimWrapper.step switches back. + gr.switch(act) + + # SIM path: 2. SIM: Step physics once + assert isinstance(self.lead_env, SimEnv) + self.lead_env.step_sim() + + # follows gym env by combinding a dict of envs into a single env obs = {} reward = 0.0 @@ -345,11 +466,22 @@ def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, boo truncated = False info = {} for key, env in self.envs.items(): - act = self._translate_pose(key, action[key], to_world=False) - ob, r, t, tr, info[key] = env.step(act) + + + if self._runs_in_sim: + # SIM path: 3. UP: Gather observations + # Resume robot greenlet. It returns the step results. + res = step_greenlets[key].switch() + ob, r, t, tr, i = res + + else: + # HARDWARE path + act = self._translate_pose(key, action[key], to_world=False) + ob, r, t, tr, info[key] = env.step(act) + + + obs[key] = self._translate_pose(key, ob, to_world=True) - # old - # obs[key], r, t, tr, info[key] = env.step(action[key]) reward += float(r) terminated = terminated or t truncated = truncated or tr @@ -358,15 +490,43 @@ def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, boo return obs, reward, terminated, truncated, info def reset( - self, seed: dict[str, int] | None = None, options: dict[str, dict[str, Any]] | None = None # type: ignore + self, *, seed: int | None = None, options: dict[str, Any] | None = None ) -> tuple[dict[str, Any], dict[str, Any]]: obs = {} info = {} - seed_ = seed if seed is not None else {key: None for key in self.envs} # type: ignore - options_ = options if options is not None else {key: None for key in self.envs} # type: ignore + seed_ = {key: seed for key in self.envs} if seed is not None else {key: None for key in self.envs} + options_ = options if options is not None else {key: None for key in self.envs} + + + reset_greenlets = {} + if self._runs_in_sim: + # SIM path: 1. DOWN: Reset each robot + for key, env in self.envs.items(): + def make_reset_gr(env_to_reset, s, o): + return greenlet(lambda: env_to_reset.reset(seed=s, options=o)) + + gr = make_reset_gr(env, seed_[key], options_[key]) + reset_greenlets[key] = gr + gr.switch() + + # SIM path: 2. SIM: reset + assert isinstance(self.lead_env, SimEnv) + self.lead_env.reset_sim() + + for key, env in self.envs.items(): - obs[key], info[key] = env.reset(seed=seed_[key], options=options_[key]) + if self._runs_in_sim: + # SIM path: 3. UP: Gather initial obs + ob, i = reset_greenlets[key].switch() + else: + # HARDWARE path + ob, i = env.reset(seed=seed_[key], options=options_[key]) + + obs[key] = self._translate_pose(key, ob, to_world=True) + info[key] = i + + return obs, info def get_wrapper_attr(self, name: str) -> Any: @@ -400,12 +560,12 @@ def __init__( max_mov: float | tuple[float, float] | None = None, ): super().__init__(env) - self.unwrapped: RobotEnv self.action_space: gym.spaces.Dict self.relative_to = relative_to + self._robot = cast(common.Robot, self.get_wrapper_attr("robot")) if ( - self.unwrapped.get_control_mode() == ControlMode.CARTESIAN_TRPY - or self.unwrapped.get_control_mode() == ControlMode.CARTESIAN_TQuat + self.get_wrapper_attr("get_control_mode")() == ControlMode.CARTESIAN_TRPY + or self.get_wrapper_attr("get_control_mode")() == ControlMode.CARTESIAN_TQuat ): if max_mov is None: max_mov = (self.DEFAULT_MAX_CART_MOV, self.DEFAULT_MAX_CART_ROT) @@ -436,7 +596,7 @@ def __init__( ) self.max_mov: float | tuple[float, float] = max_mov - if self.unwrapped.get_control_mode() == ControlMode.CARTESIAN_TRPY: + if self.get_wrapper_attr("get_control_mode")() == ControlMode.CARTESIAN_TRPY: assert isinstance(self.max_mov, tuple) self.action_space.spaces.update( get_space( @@ -444,14 +604,14 @@ def __init__( params={"cart_limits": {"max_cart_mov": self.max_mov[0], "max_angle_mov": self.max_mov[1]}}, ).spaces ) - elif self.unwrapped.get_control_mode() == ControlMode.JOINTS: + elif self.get_wrapper_attr("get_control_mode")() == ControlMode.JOINTS: self.action_space.spaces.update( get_space( LimitedJointsRelDictType, - params={"joint_limits": {"max_joint_mov": self.max_mov, "dof": get_dof(self.unwrapped.robot)}}, + params={"joint_limits": {"max_joint_mov": self.max_mov, "dof": get_dof(self._robot)}}, ).spaces ) - elif self.unwrapped.get_control_mode() == ControlMode.CARTESIAN_TQuat: + elif self.get_wrapper_attr("get_control_mode")() == ControlMode.CARTESIAN_TQuat: assert isinstance(self.max_mov, tuple) self.action_space.spaces.update( get_space( @@ -470,7 +630,7 @@ def __init__( self._last_action: common.Pose | VecType | None = None def set_origin(self, origin: common.Pose | VecType): - if self.unwrapped.get_control_mode() == ControlMode.JOINTS: + if self.get_wrapper_attr("get_control_mode")() == ControlMode.JOINTS: assert isinstance( origin, np.ndarray ), "Invalid origin type. If control mode is joints, origin must be VecType." @@ -482,10 +642,10 @@ def set_origin(self, origin: common.Pose | VecType): self._origin = copy.deepcopy(origin) def set_origin_to_current(self): - if self.unwrapped.get_control_mode() == ControlMode.JOINTS: - self._origin = self.unwrapped.robot.get_joint_position() + if self.get_wrapper_attr("get_control_mode")() == ControlMode.JOINTS: + self._origin = self._robot.get_joint_position() else: - self._origin = self.unwrapped.robot.get_cartesian_position() + self._origin = self._robot.get_cartesian_position() def reset(self, **kwargs) -> tuple[dict, dict[str, Any]]: obs, info = super().reset(**kwargs) @@ -500,10 +660,10 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: # -> could be done after the step to the state that is returned by the observation self.set_origin_to_current() action = copy.deepcopy(action) - if self.unwrapped.get_control_mode() == ControlMode.JOINTS and self.joints_key in action: + if self.get_wrapper_attr("get_control_mode")() == ControlMode.JOINTS and self.joints_key in action: assert isinstance(self._origin, np.ndarray), "Invalid origin type give the control mode." assert isinstance(self.max_mov, float) - low, high = get_joint_limits(self.unwrapped.robot) + low, high = get_joint_limits(self._robot) # TODO: should we also clip euqally for all joints? if self.relative_to == RelativeTo.LAST_STEP or self._last_action is None: limited_joints = np.clip(action[self.joints_key], -self.max_mov, self.max_mov) @@ -515,7 +675,7 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: self._last_action = limited_joints action.update(JointsDictType(joints=np.clip(self._origin + limited_joints, low, high))) - elif self.unwrapped.get_control_mode() == ControlMode.CARTESIAN_TRPY and self.trpy_key in action: + elif self.get_wrapper_attr("get_control_mode")() == ControlMode.CARTESIAN_TRPY and self.trpy_key in action: assert isinstance(self._origin, common.Pose), "Invalid origin type given the control mode." assert isinstance(self.max_mov, tuple) pose_space = cast(gym.spaces.Box, get_space(TRPYDictType).spaces[self.trpy_key]) @@ -559,7 +719,7 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: ) ) ) - elif self.unwrapped.get_control_mode() == ControlMode.CARTESIAN_TQuat and self.tquat_key in action: + elif self.get_wrapper_attr("get_control_mode")() == ControlMode.CARTESIAN_TQuat and self.tquat_key in action: assert isinstance(self._origin, common.Pose), "Invalid origin type given the control mode." assert isinstance(self.max_mov, tuple) pose_space = cast(gym.spaces.Box, get_space(TQuatDictType).spaces[self.tquat_key]) @@ -616,7 +776,6 @@ class CameraSetWrapper(ActObsInfoWrapper): def __init__(self, env, camera_set: BaseCameraSet, include_depth: bool = False): super().__init__(env) - self.unwrapped: RobotEnv self.camera_set = camera_set self.include_depth = include_depth @@ -657,7 +816,7 @@ def __init__(self, env, camera_set: BaseCameraSet, include_depth: bool = False): ) self.camera_key = get_space_keys(CameraDictType)[0] - def reset(self, seed: int | None = None, options: dict[str, Any] | None = None) -> tuple[dict, dict[str, Any]]: + def reset(self, *, seed: int | None = None, options: dict[str, Any] | None = None) -> tuple[dict, dict[str, Any]]: self.camera_set.clear_buffer() return super().reset(seed=seed, options=options) @@ -717,7 +876,6 @@ class GripperWrapper(ActObsInfoWrapper): def __init__(self, env, gripper: common.Gripper, binary: bool = True): super().__init__(env) - self.unwrapped: RobotEnv self.observation_space: gym.spaces.Dict self.observation_space.spaces.update(get_space(GripperDictType).spaces) self.action_space: gym.spaces.Dict @@ -786,7 +944,6 @@ class HandWrapper(ActObsInfoWrapper): def __init__(self, env, hand: Hand, binary: bool = True): super().__init__(env) - self.unwrapped: RobotEnv self.observation_space: gym.spaces.Dict self.action_space: gym.spaces.Dict self.binary = binary @@ -798,11 +955,11 @@ def __init__(self, env, hand: Hand, binary: bool = True): self.observation_space.spaces.update(get_space(HandVecDictType).spaces) self.action_space.spaces.update(get_space(HandVecDictType).spaces) self.hand_key = get_space_keys(HandVecDictType)[0] - self._hand = hand + self.hand = hand self._last_hand_cmd = None def reset(self, **kwargs) -> tuple[dict[str, Any], dict[str, Any]]: - self._hand.reset() + self.hand.reset() self._last_hand_cmd = None return super().reset(**kwargs) @@ -813,7 +970,7 @@ def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tupl self._last_hand_cmd if self._last_hand_cmd is not None else self.BINARY_HAND_OPEN ) else: - observation[self.hand_key] = self._hand.get_normalized_joint_poses() + observation[self.hand_key] = self.hand.get_normalized_joint_poses() info = {} return observation, info @@ -829,14 +986,14 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: if self.binary: if self._last_hand_cmd is None or self._last_hand_cmd != hand_action: if hand_action == self.BINARY_HAND_CLOSED: - self._hand.grasp() + self.hand.grasp() else: - self._hand.open() + self.hand.open() else: - self._hand.set_normalized_joint_poses(hand_action) + self.hand.set_normalized_joint_poses(hand_action) self._last_hand_cmd = hand_action del action[self.hand_key] return action def close(self): - self._hand.close() + self.hand.close() diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index af189fa4..69e7fcf9 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -18,7 +18,8 @@ MultiRobotWrapper, RelativeActionSpace, RelativeTo, - RobotEnv, + RobotWrapper, + SimEnv, ) from rcs.envs.sim import ( GripperWrapperSim, @@ -27,7 +28,6 @@ RandomCubePos, RandomObjectPos, RobotSimWrapper, - SimWrapper, ) from rcs.envs.utils import default_sim_gripper_cfg, default_sim_robot_cfg @@ -54,7 +54,6 @@ def __call__( # type: ignore cameras: dict[str, SimCameraConfig] | None = None, max_relative_movement: float | tuple[float, float] | None = None, relative_to: RelativeTo = RelativeTo.LAST_STEP, - sim_wrapper: Type[SimWrapper] | None = None, ) -> gym.Env: """ Creates a simulation environment for the FR3 robot. @@ -72,8 +71,6 @@ def __call__( # type: ignore translational movement in meters. If tuple, it restricts both translational (in meters) and rotational (in radians) movements. If None, no restriction is applied. relative_to (RelativeTo): Specifies whether the movement is relative to a configured origin or the last step. - sim_wrapper (Type[SimWrapper] | None): Wrapper to be applied before the simulation wrapper. This is useful - for reset management e.g. resetting objects in the scene with correct observations. Defaults to None. Returns: gym.Env: The configured simulation environment for the FR3 robot. @@ -88,7 +85,8 @@ def __call__( # type: ignore # ik = rcs_robotics_library._core.rl.RoboticsLibraryIK(robot_cfg.kinematic_model_path) robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) - env: gym.Env = RobotEnv(robot, control_mode) + env = SimEnv(simulation) + env = RobotWrapper(env, robot, control_mode) assert not ( hand_cfg is not None and gripper_cfg is not None ), "Hand and gripper configurations cannot be used together." @@ -96,7 +94,7 @@ def __call__( # type: ignore if hand_cfg is not None and isinstance(hand_cfg, rcs.sim.SimTilburgHandConfig): hand = sim.SimTilburgHand(simulation, hand_cfg) env = HandWrapper(env, hand, binary=True) - env = HandWrapperSim(env, hand) + env = HandWrapperSim(env) if gripper_cfg is not None and isinstance(gripper_cfg, rcs.sim.SimGripperConfig): gripper = sim.SimGripper(simulation, gripper_cfg) @@ -104,10 +102,10 @@ def __call__( # type: ignore else: gripper = None - env = RobotSimWrapper(env, simulation, sim_wrapper) + env = RobotSimWrapper(env) if gripper is not None: - env = GripperWrapperSim(env, gripper) + env = GripperWrapperSim(env) if cameras is not None: camera_set = typing.cast( @@ -146,7 +144,6 @@ def __call__( # type: ignore cameras: dict[str, SimCameraConfig] | None = None, max_relative_movement: float | tuple[float, float] | None = None, relative_to: RelativeTo = RelativeTo.LAST_STEP, - sim_wrapper: Type[SimWrapper] | None = None, robot2world: dict[str, rcs.common.Pose] | None = None, ) -> gym.Env: @@ -166,19 +163,24 @@ def __call__( # type: ignore envs = {} for key, mid in name2id.items(): - env: gym.Env = RobotEnv(robots[key], control_mode) + env = SimEnv(simulation) + env = RobotWrapper(env, robots[key], control_mode) if gripper_cfg is not None: gripper_cfg_copy = copy.copy(gripper_cfg) gripper_cfg_copy.add_postfix("_" + mid) gripper = rcs.sim.SimGripper(simulation, gripper_cfg_copy) env = GripperWrapper(env, gripper, binary=True) + env = RobotSimWrapper(env) + + if gripper_cfg is not None: + env = GripperWrapperSim(env) # type: ignore[possibly-undefined] + if relative_to != RelativeTo.NONE: env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) envs[key] = env env = MultiRobotWrapper(envs, robot2world) - env = RobotSimWrapper(env, simulation, sim_wrapper) if cameras is not None: camera_set = typing.cast( BaseCameraSet, SimCameraSet(simulation, cameras, physical_units=True, render_on_demand=True) @@ -249,8 +251,8 @@ def __call__( # type: ignore cameras=cameras, max_relative_movement=(0.2, np.deg2rad(45)) if delta_actions else None, relative_to=RelativeTo.LAST_STEP, - sim_wrapper=random_env, # type: ignore ) + env_rel = random_env(env_rel) if mode == "gripper": env_rel = PickCubeSuccessWrapper(env_rel, cube_joint_name=obj_joint_name) diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index 752c75c9..e78dab30 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -1,18 +1,13 @@ import logging -from typing import Any, SupportsFloat, Type, cast +from typing import Any, cast import gymnasium as gym import numpy as np +from rcs._core.common import RobotPlatform from rcs.envs.base import ( - ControlMode, GripperWrapper, - HandWrapper, - MultiRobotWrapper, - RobotEnv, ) from rcs.envs.space_utils import ActObsInfoWrapper -from rcs.envs.utils import default_sim_robot_cfg, default_sim_tilburg_hand_cfg -from rcs.utils import SimpleFrameRate import rcs from rcs import sim @@ -21,43 +16,20 @@ logger.setLevel(logging.INFO) -class SimWrapper(gym.Wrapper): - """A sub class of this wrapper can be passed to FR3Sim to assure that its code is called before - step_until_convergence() is called. - """ - - def __init__(self, env: gym.Env, simulation: sim.Sim): +class RobotSimWrapper(ActObsInfoWrapper): + def __init__(self, env): super().__init__(env) - self.unwrapped: RobotEnv - assert isinstance(self.unwrapped.robot, sim.SimRobot), "Robot must be a sim.SimRobot instance." - self.sim = simulation + assert self.env.get_wrapper_attr("PLATFORM") == RobotPlatform.SIMULATION, "Base environment must be simulation." + assert isinstance(self.get_wrapper_attr("robot"), sim.SimRobot), "Robot must be a sim.SimRobot instance." + self.sim_robot = cast(sim.SimRobot, self.get_wrapper_attr("robot")) + self.sim = cast(sim.Sim, self.get_wrapper_attr("sim")) + def action(self, action: dict[str, Any]) -> dict[str, Any]: + self.sim_robot.clear_collision_flag() + return action -class RobotSimWrapper(gym.Wrapper): - def __init__(self, env, simulation: sim.Sim, sim_wrapper: Type[SimWrapper] | None = None): - self.sim_wrapper = sim_wrapper - if sim_wrapper is not None: - env = sim_wrapper(env, simulation) - super().__init__(env) - self.unwrapped: RobotEnv - assert isinstance(self.unwrapped.robot, sim.SimRobot), "Robot must be a sim.SimRobot instance." - self.sim_robot = cast(sim.SimRobot, self.unwrapped.robot) - self.sim = simulation - cfg = self.sim.get_config() - self.frame_rate = SimpleFrameRate(1 / cfg.frequency, "RobotSimWrapper") - - def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, bool, dict]: - obs, _, _, _, info = super().step(action) - cfg = self.sim.get_config() - if cfg.async_control: - self.sim.step(round(1 / cfg.frequency / self.sim.model.opt.timestep)) - if cfg.realtime: - self.frame_rate.frame_rate = 1 / cfg.frequency - self.frame_rate() - else: - self.sim_robot.clear_collision_flag() - self.sim.step_until_convergence() + def observation(self, observation: dict, info: dict[str, Any]) -> tuple[dict[str, Any], dict[str, Any]]: state = self.sim_robot.get_state() if "collision" not in info: info["collision"] = state.collision @@ -65,67 +37,136 @@ def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, boo info["collision"] = info["collision"] or state.collision info["ik_success"] = state.ik_success info["is_sim_converged"] = self.sim.is_converged() - # truncate episode if collision - obs.update(self.unwrapped.get_obs()) - return obs, 0, False, info["collision"] or not state.ik_success, info - # return obs, 0, False, False, info - - def reset( - self, *, seed: int | None = None, options: dict[str, Any] | None = None - ) -> tuple[dict[str, Any], dict[str, Any]]: - self.sim.reset() - obs, info = super().reset(seed=seed, options=options) - self.sim.step(1) - # todo: an obs method that is recursive over wrappers would be needed - obs.update(self.unwrapped.get_obs()) - return obs, info - - -class MultiSimRobotWrapper(gym.Wrapper): - """Wraps a dictionary of environments to allow for multi robot control.""" - - def __init__(self, env: MultiRobotWrapper, simulation: sim.Sim): - super().__init__(env) - self.env: MultiRobotWrapper - self.sim = simulation - self.sim_robots = cast(dict[str, sim.SimRobot], {key: e.robot for key, e in self.env.unwrapped_multi.items()}) + return observation, info - def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, bool, dict]: - _, _, _, _, info = super().step(action) - self.sim.step_until_convergence() - info["is_sim_converged"] = self.sim.is_converged() - for key in self.envs.envs.items(): - state = self.sim_robots[key].get_state() - info[key]["collision"] = state.collision - info[key]["ik_success"] = state.ik_success + # def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, bool, dict]: + # self.sim_robot.clear_collision_flag() + # obs, _, _, _, info = super().step(action) - obs = {key: env.get_obs() for key, env in self.env.unwrapped_multi.items()} - truncated = np.all([info[key]["collision"] or info[key]["ik_success"] for key in info]) - return obs, 0.0, False, bool(truncated), info + # state = self.sim_robot.get_state() + # if "collision" not in info: + # info["collision"] = state.collision + # else: + # info["collision"] = info["collision"] or state.collision + # info["ik_success"] = state.ik_success + # info["is_sim_converged"] = self.sim.is_converged() + # # truncate episode if collision + # return obs, 0, False, info["collision"] or not state.ik_success, info - def reset( # type: ignore - self, *, seed: dict[str, int | None] | None = None, options: dict[str, Any] | None = None + def reset( + self, *, seed: int | None = None, options: dict[str, Any] | None = None ) -> tuple[dict[str, Any], dict[str, Any]]: - if seed is None: - seed = dict.fromkeys(self.env.envs) - if options is None: - options = {key: {} for key in self.env.envs} - obs = {} - info = {} - self.sim.reset() - for key, env in self.env.envs.items(): - _, info[key] = env.reset(seed=seed[key], options=options[key]) - self.sim.step(1) - for key, env in self.env.unwrapped_multi.items(): - obs[key] = cast(dict, env.get_obs()) - return obs, info + self.sim_robot.clear_collision_flag() + return super().reset(seed=seed, options=options) + + +# class MultiSimRobotWrapper(gym.Wrapper): +# """Wraps a dictionary of environments to allow for multi robot control.""" + +# def __init__(self, env: MultiRobotWrapper, simulation: sim.Sim): +# super().__init__(env) +# self.env: MultiRobotWrapper +# self.sim = simulation +# self.sim_robots = cast(dict[str, sim.SimRobot], {key: e.robot for key, e in self.env.unwrapped_multi.items()}) +# self._inject_main_greenlet() + +# def _inject_main_greenlet(self): +# main_gr = getcurrent() +# for env_item in self.env.envs.values(): +# curr = env_item +# while True: +# if isinstance(curr, RobotSimWrapper): +# curr.main_greenlet = main_gr +# break +# if not hasattr(curr, "env"): +# break +# curr = curr.env + +# def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, bool, dict]: +# # 1. DOWN: Set actions for all robots +# step_greenlets = {} +# for key, env in self.env.envs.items(): + +# def make_step_gr(env_to_step): +# return greenlet(env_to_step.step) + +# gr = make_step_gr(env) +# step_greenlets[key] = gr + +# # Translate action +# act = self.env._translate_pose(key, action[key], to_world=False) + +# # Switch to robot greenlet. It will run until RobotSimWrapper.step switches back. +# gr.switch(act) + +# # 2. SIM: Step physics once +# self.sim.step_until_convergence() + +# # 3. UP: Gather observations +# obs = {} +# reward = 0.0 +# terminated = False +# truncated = False +# info = {} + +# for key in self.env.envs: +# # Resume robot greenlet. It returns the step results. +# res = step_greenlets[key].switch() +# ob, r, t, tr, i = res + +# # Translate observation back to world coordinates +# obs[key] = self.env._translate_pose(key, ob, to_world=True) +# reward += float(r) +# terminated = terminated or t +# truncated = truncated or tr +# info[key] = i +# info[key]["terminated"] = t +# info[key]["truncated"] = tr +# info[key]["is_sim_converged"] = self.sim.is_converged() + +# return obs, reward, terminated, truncated, info + +# def reset( # type: ignore +# self, *, seed: dict[str, int | None] | None = None, options: dict[str, Any] | None = None +# ) -> tuple[dict[str, Any], dict[str, Any]]: +# if seed is None: +# seed = dict.fromkeys(self.env.envs) +# if options is None: +# options = {key: {} for key in self.env.envs} +# obs = {} +# info = {} +# self.sim.reset() + +# # 1. DOWN: Reset each robot +# reset_greenlets = {} +# for key, env in self.env.envs.items(): + +# def make_reset_gr(env_to_reset, s, o): +# return greenlet(lambda: env_to_reset.reset(seed=s, options=o)) + +# gr = make_reset_gr(env, seed[key], options[key]) +# reset_greenlets[key] = gr +# gr.switch() + +# # 2. SIM: Initial step +# self.sim.step(1) + +# # 3. UP: Gather initial obs +# for key in self.env.envs: +# ob, i = reset_greenlets[key].switch() +# obs[key] = self.env._translate_pose(key, ob, to_world=True) +# info[key] = i + +# return obs, info class GripperWrapperSim(ActObsInfoWrapper): - def __init__(self, env, gripper: sim.SimGripper): + def __init__(self, env): super().__init__(env) - self._gripper = gripper + assert self.env.get_wrapper_attr("PLATFORM") == RobotPlatform.SIMULATION, "Base environment must be simulation." + assert isinstance(self.get_wrapper_attr("gripper"), sim.SimGripper), "Gripper must be a sim.SimGripper instance." + self._gripper = cast(sim.SimGripper, self.get_wrapper_attr("gripper")) def action(self, action: dict[str, Any]) -> dict[str, Any]: self._gripper.clear_collision_flag() @@ -139,11 +180,19 @@ def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tupl info["is_grasped"] = self._gripper.get_normalized_width() > 0.01 and self._gripper.get_normalized_width() < 0.99 return observation, info + def reset( + self, *, seed: int | None = None, options: dict[str, Any] | None = None + ) -> tuple[dict[str, Any], dict[str, Any]]: + self._gripper.clear_collision_flag() + return super().reset(seed=seed, options=options) + class HandWrapperSim(ActObsInfoWrapper): - def __init__(self, env, hand: sim.SimTilburgHand): + def __init__(self, env): super().__init__(env) - self._hand = hand + assert self.env.get_wrapper_attr("PLATFORM") == RobotPlatform.SIMULATION, "Base environment must be simulation." + assert isinstance(self.get_wrapper_attr("hand"), sim.SimTilburgHand), "Hand must be a sim.SimTilburgHand instance." + self._hand = cast(sim.SimTilburgHand, self.get_wrapper_attr("hand")) def action(self, action: dict[str, Any]) -> dict[str, Any]: if isinstance(action["hand"], int | float): @@ -162,140 +211,9 @@ def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tupl return observation, info -class CollisionGuard(gym.Wrapper[dict[str, Any], dict[str, Any], dict[str, Any], dict[str, Any]]): - """ - - Gripper Wrapper has to be added before this as it removes the gripper action - - RelativeActionSpace has to be added after this as it changes the input space, and the input expects absolute actions - """ - - def __init__( - self, - env: gym.Env, - simulation: sim.Sim, - collision_env: gym.Env, - check_home_collision: bool = True, - to_joint_control: bool = False, - sim_gui: bool = True, - truncate_on_collision: bool = True, - ): - super().__init__(env) - self.unwrapped: RobotEnv - self.collision_env = collision_env - self.sim = simulation - self.last_obs: tuple[dict[str, Any], dict[str, Any]] | None = None - self._logger = logging.getLogger(__name__) - self.check_home_collision = check_home_collision - self.to_joint_control = to_joint_control - self.truncate_on_collision = truncate_on_collision - if to_joint_control: - assert ( - self.unwrapped.get_unwrapped_control_mode(-2) == ControlMode.JOINTS - ), "Previous control mode must be joints" - # change action space - self.action_space = self.collision_env.action_space - if sim_gui: - self.sim.open_gui() - - def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], SupportsFloat, bool, bool, dict[str, Any]]: - self.collision_env.get_wrapper_attr("robot").set_joints_hard(self.unwrapped.robot.get_joint_position()) - _, _, _, _, info = self.collision_env.step(action) - - if self.to_joint_control: - fr3_env = self.collision_env.unwrapped - assert isinstance(fr3_env, RobotEnv), "Collision env must be an RobotEnv instance." - action[self.unwrapped.joints_key] = fr3_env.robot.get_joint_position() - - if info["collision"]: - self._logger.warning("Collision detected! %s", info) - action[self.unwrapped.joints_key] = self.unwrapped.robot.get_joint_position() - if self.truncate_on_collision: - if self.last_obs is None: - msg = "Collision detected in the first step!" - raise RuntimeError(msg) - return self.last_obs[0], 0, True, True, info - - obs, reward, done, truncated, info = super().step(action) - self.last_obs = obs, info - return obs, reward, done, truncated, info - - def reset( - self, *, seed: int | None = None, options: dict[str, Any] | None = None - ) -> tuple[dict[str, Any], dict[str, Any]]: - # check if move to home is collision free - if self.check_home_collision: - self.collision_env.get_wrapper_attr("sim_robot").move_home() - self.collision_env.get_wrapper_attr("sim").step_until_convergence() - state = self.collision_env.get_wrapper_attr("sim_robot").get_state() - if state.collision or not state.ik_success: - msg = "Collision detected while moving to home position!" - raise RuntimeError(msg) - else: - self.collision_env.get_wrapper_attr("sim_robot").reset() - obs, info = super().reset(seed=seed, options=options) - self.last_obs = obs, info - return obs, info - - @classmethod - def env_from_xml_paths( - cls, - env: gym.Env, - mjmld: str, - cg_kinematics_path: str, - id: str = "_0", - gripper: bool = True, - hand: bool = False, - check_home_collision: bool = True, - tcp_offset: rcs.common.Pose | None = None, - control_mode: ControlMode | None = None, - sim_gui: bool = True, - truncate_on_collision: bool = True, - ) -> "CollisionGuard": - # TODO: remove urdf and use mjcf - # TODO: this needs to support non FR3 robots - assert isinstance(env.unwrapped, RobotEnv) - simulation = sim.Sim(mjmld) - cfg = default_sim_robot_cfg(mjmld, id) - ik = rcs.common.Pin(cg_kinematics_path, cfg.attachment_site, False) - if tcp_offset is not None: - cfg.tcp_offset = tcp_offset - robot = rcs.sim.SimRobot(simulation, ik, cfg) - to_joint_control = False - if control_mode is not None: - if control_mode != env.unwrapped.get_control_mode(): - assert ( - env.unwrapped.get_control_mode() == ControlMode.JOINTS - ), "A different control mode between collision guard and base env can only be used if the base env uses joint control" - env.unwrapped.override_control_mode(control_mode) - to_joint_control = True - else: - control_mode = env.unwrapped.get_control_mode() - c_env: gym.Env = RobotEnv(robot, control_mode) - c_env = RobotSimWrapper(c_env, simulation) - if gripper: - gripper_cfg = sim.SimGripperConfig() - gripper_cfg.add_postfix(id) - fh = sim.SimGripper(simulation, gripper_cfg) - c_env = GripperWrapper(c_env, fh) - c_env = GripperWrapperSim(c_env, fh) - if hand: - hand_cfg = default_sim_tilburg_hand_cfg() - # hand_cfg.add_postfix(id) - th = sim.SimTilburgHand(simulation, hand_cfg) - c_env = HandWrapper(c_env, th) - c_env = HandWrapperSim(c_env, th) - - return cls( - env=env, - simulation=simulation, - collision_env=c_env, - check_home_collision=check_home_collision, - to_joint_control=to_joint_control, - sim_gui=sim_gui, - truncate_on_collision=truncate_on_collision, - ) -class RandomObjectPos(SimWrapper): +class RandomObjectPos(gym.Wrapper): """ Wrapper to randomly re-place an object in the lab environments. Given the object's joint name and initial pose, its x, y coordinates are randomized, while z remains fixed. @@ -312,7 +230,6 @@ class RandomObjectPos(SimWrapper): def __init__( self, env: gym.Env, - simulation: sim.Sim, joint_name: str, init_object_pose: rcs.common.Pose, include_position: bool = True, @@ -322,7 +239,7 @@ def __init__( x_offset: float = 0.1, y_offset: float = 0.1, ): - super().__init__(env, simulation) + super().__init__(env) self.joint_name = joint_name self.init_object_pose = init_object_pose self.include_position = include_position @@ -344,7 +261,6 @@ def reset( print("Got random object pos!\n", self.init_object_pose) del options["RandomObjectPos.init_object_pose"] obs, info = super().reset(seed=seed, options=options) - self.sim.step(1) pos_z = self.init_object_pose.translation()[2] if self.include_position: @@ -357,7 +273,7 @@ def reset( quat = self.init_object_pose.rotation_q() # xyzw format if self.include_rotation: random_z_rotation = (np.random.random() - 0.5) * (0.7071068 * 2) - self.sim.data.joint(self.joint_name).qpos = [ + self.get_wrapper_attr("sim").data.joint(self.joint_name).qpos = [ pos_x, pos_y, pos_z, @@ -367,16 +283,19 @@ def reset( quat[2] + random_z_rotation, ] else: - self.sim.data.joint(self.joint_name).qpos = [pos_x, pos_y, pos_z, quat[3], quat[0], quat[1], quat[2]] + self.get_wrapper_attr("sim").data.joint(self.joint_name).qpos = [pos_x, pos_y, pos_z, quat[3], quat[0], quat[1], quat[2]] return obs, info -class RandomCubePos(SimWrapper): - """Wrapper to randomly place cube in the lab environments.""" +class RandomCubePos(gym.Wrapper): + """Wrapper to randomly place cube in the lab environments. + + Works only for single robot + """ - def __init__(self, env: gym.Env, simulation: sim.Sim, include_rotation: bool = False, cube_joint_name="box_joint"): - super().__init__(env, simulation) + def __init__(self, env: gym.Env, include_rotation: bool = False, cube_joint_name="box_joint"): + super().__init__(env) self.include_rotation = include_rotation self.cube_joint_name = cube_joint_name @@ -384,19 +303,18 @@ def reset( self, *, seed: int | None = None, options: dict[str, Any] | None = None ) -> tuple[dict[str, Any], dict[str, Any]]: obs, info = super().reset(seed=seed, options=options) - self.sim.step(1) iso_cube = np.array([0.498, 0.0, 0.226]) iso_cube_pose = rcs.common.Pose(translation=np.array(iso_cube), rpy_vector=np.array([0, 0, 0])) # type: ignore - iso_cube = self.unwrapped.robot.to_pose_in_world_coordinates(iso_cube_pose).translation() + iso_cube = self.get_wrapper_attr("robot").to_pose_in_world_coordinates(iso_cube_pose).translation() pos_z = 0.0288 pos_x = iso_cube[0] + np.random.random() * 0.2 - 0.1 pos_y = iso_cube[1] + np.random.random() * 0.2 - 0.1 if self.include_rotation: - self.sim.data.joint(self.cube_joint_name).qpos = [pos_x, pos_y, pos_z, 2 * np.random.random() - 1, 0, 0, 1] + self.get_wrapper_attr("sim").data.joint(self.cube_joint_name).qpos = [pos_x, pos_y, pos_z, 2 * np.random.random() - 1, 0, 0, 1] else: - self.sim.data.joint(self.cube_joint_name).qpos = [pos_x, pos_y, pos_z, 0, 0, 0, 1] + self.get_wrapper_attr("sim").data.joint(self.cube_joint_name).qpos = [pos_x, pos_y, pos_z, 0, 0, 0, 1] return obs, info @@ -411,15 +329,15 @@ class PickCubeSuccessWrapper(gym.Wrapper): - whether the arm is standing still once the task is solved. """ - def __init__(self, env, cube_joint_name="box_joint"): + def __init__(self, env, cube_geom_name="box_geom"): super().__init__(env) - self.unwrapped: RobotEnv - assert isinstance(self.unwrapped.robot, sim.SimRobot), "Robot must be a sim.SimRobot instance." - self.sim = env.get_wrapper_attr("sim") - self.cube_geom_name = "box_geom" - self.home_pose = self.unwrapped.robot.get_cartesian_position() + assert isinstance(self.get_wrapper_attr("robot"), sim.SimRobot), "Robot must be a sim.SimRobot instance." + self._robot = cast(sim.SimRobot, self.get_wrapper_attr("robot")) + self.sim = self.env.get_wrapper_attr("sim") + self.cube_geom_name = cube_geom_name + self.home_pose = self._robot.get_cartesian_position() self._gripper_closing = 0 - self._gripper = self.get_wrapper_attr("_gripper") + self._gripper = self.get_wrapper_attr("gripper") def step(self, action: dict[str, Any]): # type: ignore obs, reward, _, truncated, info = super().step(action) @@ -432,9 +350,9 @@ def step(self, action: dict[str, Any]): # type: ignore else: self._gripper_closing = 0 cube_pose = rcs.common.Pose(translation=self.sim.data.geom(self.cube_geom_name).xpos) - cube_pose = self.unwrapped.robot.to_pose_in_robot_coordinates(cube_pose) + cube_pose = self._robot.to_pose_in_robot_coordinates(cube_pose) tcp_to_obj_dist = np.linalg.norm( - cube_pose.translation() - self.unwrapped.robot.get_cartesian_position().translation() + cube_pose.translation() - self._robot.get_cartesian_position().translation() ) obj_to_goal_dist = 0.10 - min(cube_pose.translation()[-1], 0.10) obj_to_goal_dist = np.linalg.norm(cube_pose.translation() - self.home_pose.translation()) @@ -458,7 +376,7 @@ def step(self, action: dict[str, Any]): # type: ignore def reset(self, *, seed: int | None = None, options: dict[str, Any] | None = None): obs, info = super().reset() - self.home_pose = self.unwrapped.robot.get_cartesian_position() + self.home_pose = self._robot.get_cartesian_position() return obs, info diff --git a/python/rcs/ompl/mj_ompl.py b/python/rcs/ompl/mj_ompl.py index 7112b255..6d865cea 100644 --- a/python/rcs/ompl/mj_ompl.py +++ b/python/rcs/ompl/mj_ompl.py @@ -7,7 +7,7 @@ from ompl import base as ob from ompl import geometric as og from rcs._core.common import Pose -from rcs.envs.base import RobotEnv +import gymnasium as gym DEFAULT_PLANNING_TIME = 5.0 # Default time allowed for planning in seconds INTERPOLATE_NUM = 500 # Number of points to interpolate between start and goal states @@ -60,7 +60,7 @@ class MjORobot: def __init__( self, robot_name: str, - env: RobotEnv, + env: gym.Env, njoints: int = 7, robot_tcp: Pose | None = None, robot_xml_name: str = "", @@ -72,13 +72,13 @@ def __init__( ): """ Initialize the robot object with the given parameters. - It is essentially a thin wrapper around the RobotEnv (i.e. MuJoCo variables), + It is essentially a thin wrapper around the RobotWrapper (i.e. MuJoCo variables), for easily extracting the relevant bodies, joints, etc. from the MuJoCo model. Anything that directly calls MuJoCo functions should be done through this class. Parameters: - robot_name: Name of the robot. - - env: The RobotEnv environment in which the robot operates. + - env: The gym environment with RobotWrapper in which the robot operates. - njoints: Number of joints in the robot. - robot_xml_name: Path to the robot's XML file. This file will be used to query the s of the robot to get collision checking info. @@ -282,7 +282,7 @@ def ik(self, pose, q0=None): Returns: numpy.ndarray: Joint positions that achieve the desired pose, or None if no solution is found. """ - return self.env.unwrapped.robot.get_ik().inverse(pose, q0, self.tcp_offset) # type: ignore[attr-defined] + return self.env.get_wrapper_attr("robot").get_ik().inverse(pose, q0, self.tcp_offset) # type: ignore[attr-defined] class MjOStateSpace(ob.RealVectorStateSpace): @@ -312,7 +312,7 @@ def set_state_sampler(self, state_sampler): class MjOMPL: def __init__( self, - robot_env: RobotEnv, + robot_env: gym.Env, robot_name: str, robot_xml_name: str, robot_root_name: str, @@ -328,12 +328,12 @@ def __init__( """ Initialize the OMPL planner with the given parameters. Besides setting up the OMPL planning context, it instantiates an MjORobot instance, - which is a thin wrapper around the RobotEnv (i.e. MuJoCo variables), + which is a thin wrapper around the RobotWrapper (i.e. MuJoCo variables), for easily extracting the relevant bodies, joints, etc. from the MuJoCo model. Parameters: - - robot_env: The RobotEnv environment in which the robot operates. + - robot_env: The environment with RobotWrapper in which the robot operates. - robot_name: Name of the robot. - robot_xml_name: Path to the robot's XML file. diff --git a/python/rcs/operator/interface.py b/python/rcs/operator/interface.py index e967d60e..e7e6b3c0 100644 --- a/python/rcs/operator/interface.py +++ b/python/rcs/operator/interface.py @@ -180,7 +180,7 @@ def environment_step_loop(self): # TODO the following is a dict and can thus not easily be used like this # and self.env.get_wrapper_attr("relative_to") == RelativeTo.CONFIGURED_ORIGIN ), "both robot env and operator must be configured to relative_to.CONFIGURED_ORIGIN" - self.env.get_wrapper_attr("envs")[robot].set_origin_to_current() + self.env.get_wrapper_attr("envs")[robot].get_wrapper_attr("set_origin_to_current")() # 2. Step the Environment actions = self.operator.consume_action() diff --git a/python/rcs/rpc/client.py b/python/rcs/rpc/client.py index c52f2ad2..2ab77a97 100644 --- a/python/rcs/rpc/client.py +++ b/python/rcs/rpc/client.py @@ -18,8 +18,8 @@ def step(self, action): def reset(self, **kwargs): return self.server.reset(**kwargs) - def get_obs(self): - return self.server.get_obs() + def get_robot_obs(self): + return self.server.get_robot_obs() @property def unwrapped(self): diff --git a/python/rcs/rpc/server.py b/python/rcs/rpc/server.py index ed79cb9b..029aca47 100644 --- a/python/rcs/rpc/server.py +++ b/python/rcs/rpc/server.py @@ -24,14 +24,9 @@ def reset(self, **kwargs): return super().reset(**kwargs) @rpyc.exposed - def get_obs(self): + def get_robot_obs(self): """Get the current observation using the Wrapper base class if available.""" - if hasattr(super(), "get_obs"): - return super().get_obs() - if hasattr(self.env, "get_obs"): - return self.env.get_obs() - error = "The environment does not have a get_obs method." - raise NotImplementedError(error) + return self.get_wrapper_attr("get_robot_obs")() @rpyc.exposed def unwrapped(self): diff --git a/python/tests/test_multi_sim.py b/python/tests/test_multi_sim.py new file mode 100644 index 00000000..55b301be --- /dev/null +++ b/python/tests/test_multi_sim.py @@ -0,0 +1,113 @@ +"""Tests for multi-robot simulation with greenlet-based step coordination.""" + +import numpy as np +import pytest +from rcs._core.sim import SimConfig +from rcs.envs.base import ControlMode, JointsDictType +from rcs.envs.creators import SimMultiEnvCreator +from rcs.envs.utils import default_sim_robot_cfg + +SCENE = "fr3_dual_arm" +ROBOT2ID = {"left": "0", "right": "1"} + + +@pytest.fixture() +def multi_env(): + # idx="" so SimMultiEnvCreator can add the per-robot postfix ("_0", "_1") itself + robot_cfg = default_sim_robot_cfg(SCENE, idx="") + sim_cfg = SimConfig() + sim_cfg.async_control = False + env = SimMultiEnvCreator()( + name2id=ROBOT2ID, + robot_cfg=robot_cfg, + control_mode=ControlMode.JOINTS, + gripper_cfg=None, + sim_cfg=sim_cfg, + max_relative_movement=None, + ) + yield env + env.close() + + +class TestMultiSimRobotWrapper: + def test_reset_returns_obs_for_all_robots(self, multi_env): + obs, info = multi_env.reset() + assert set(obs.keys()) == {"left", "right"} + for key in ROBOT2ID: + assert "joints" in obs[key], f"Missing joints in obs[{key!r}]" + assert "tquat" in obs[key], f"Missing tquat in obs[{key!r}]" + assert len(obs[key]["joints"]) == 7 + + def test_double_reset(self, multi_env): + multi_env.reset() + obs, info = multi_env.reset() + assert set(obs.keys()) == {"left", "right"} + + def test_step_returns_obs_for_all_robots(self, multi_env): + obs0, _ = multi_env.reset() + actions = {key: JointsDictType(joints=obs0[key]["joints"]) for key in ROBOT2ID} + obs, reward, terminated, truncated, info = multi_env.step(actions) + assert set(obs.keys()) == {"left", "right"} + assert set(info.keys()) == {"left", "right"} + for key in ROBOT2ID: + assert "joints" in obs[key] + assert "ik_success" in info[key] + assert "collision" in info[key] + + def test_multiple_reset_step_cycles(self, multi_env): + """Reset → step → reset → step should not crash or produce stale obs.""" + for cycle in range(2): + obs, info = multi_env.reset() + assert set(obs.keys()) == set(ROBOT2ID) + actions = {key: JointsDictType(joints=obs[key]["joints"]) for key in ROBOT2ID} + obs2, _, _, _, info2 = multi_env.step(actions) + assert set(obs2.keys()) == set(ROBOT2ID) + for key in ROBOT2ID: + assert info2[key]["ik_success"], f"IK failed on cycle {cycle} for {key!r}" + + def test_sim_stepped_once_per_multi_step(self, multi_env): + """ + Verify the sim is stepped ONCE per multi-step call, not once per robot. + With the same absolute target across two sequential steps, the second + step should produce the same or smaller change than the first step + (i.e. the robot is not being double-stepped). + """ + obs0, _ = multi_env.reset() + home = {key: obs0[key]["joints"].copy() for key in ROBOT2ID} + + # First step from home — physics moves the robot some amount + actions = {key: JointsDictType(joints=home[key]) for key in ROBOT2ID} + obs1, _, _, _, _ = multi_env.step(actions) + delta1 = max(np.linalg.norm(obs1[k]["joints"] - home[k]) for k in ROBOT2ID) + + # Second step with same target — robot is now closer to or past target. + # If the sim were stepped twice per call, delta2 would be >> delta1. + obs2, _, _, _, info = multi_env.step(actions) + delta2 = max(np.linalg.norm(obs2[k]["joints"] - home[k]) for k in ROBOT2ID) + + for key in ROBOT2ID: + assert info[key]["ik_success"], f"IK failed for {key!r}" + # delta2 should not be dramatically larger than delta1 (at most 3×) + assert ( + delta2 < delta1 * 3 + ), f"Second step drifted {delta2:.3f} vs first {delta1:.3f} — sim may be double-stepped" + + def test_robots_are_independent(self, multi_env): + """ + Moving one robot (joint 0) while holding the other should produce + clearly different outcomes on joint 0 for the two robots. + """ + obs0, _ = multi_env.reset() + # Apply a large delta to 'right' joint 0; 'left' holds home + big_delta_j0 = 0.3 + actions = { + "left": JointsDictType(joints=obs0["left"]["joints"].copy()), + "right": JointsDictType(joints=obs0["right"]["joints"] + np.array([big_delta_j0, 0, 0, 0, 0, 0, 0])), + } + obs, _, _, _, info = multi_env.step(actions) + right_j0_change = obs["right"]["joints"][0] - obs0["right"]["joints"][0] + left_j0_change = obs["left"]["joints"][0] - obs0["left"]["joints"][0] + assert right_j0_change > 0.01, f"Right robot joint 0 did not move toward target: change={right_j0_change:.4f}" + assert abs(left_j0_change) < abs( + right_j0_change + ), f"Left robot joint 0 moved as much as right: left={left_j0_change:.4f}, right={right_j0_change:.4f}" diff --git a/python/tests/test_rpc.py b/python/tests/test_rpc.py index 83f00caf..a1b3bdfe 100644 --- a/python/tests/test_rpc.py +++ b/python/tests/test_rpc.py @@ -188,7 +188,7 @@ def test_step(self): def test_get_obs(self): self.client.reset() - obs2 = self.client.get_obs() + obs2 = self.client.get_robot_obs() assert obs2 is not None, "get_obs did not return an observation" def test_unwrapped(self): diff --git a/python/tests/test_sim_envs.py b/python/tests/test_sim_envs.py index c997473d..d00eb49c 100644 --- a/python/tests/test_sim_envs.py +++ b/python/tests/test_sim_envs.py @@ -150,32 +150,7 @@ def test_collision_trpy(self, cfg, gripper_cfg): obs, _, _, _, info = env.step(collision_action) self.assert_collision(info) - # def test_collision_guard_trpy(self, cfg, gripper_cfg): - # """ - # Check that an obvious collision is detected by the CollisionGuard - # """ - # # env creation - # env = SimEnvCreator()( - # ControlMode.CARTESIAN_TRPY, - # cfg, - # gripper_cfg=gripper_cfg, - # collision_guard=True, - # cameras=None, - # max_relative_movement=None, - # ) - # obs, _ = env.reset() - # unwrapped = cast(RobotEnv, env.unwrapped) - # p1: np.ndarray = unwrapped.robot.get_joint_position() - # # an obvious below ground collision action - # obs["xyzrpy"][0] = 0.4 - # obs["xyzrpy"][2] = -0.05 - # collision_action = TRPYDictType(xyzrpy=obs["xyzrpy"]) - # collision_action.update(GripperDictType(gripper=np.array([0.0]))) - # _, _, _, _, info = env.step(collision_action) - # p2: np.ndarray = unwrapped.robot.get_joint_position() - # self.assert_collision(info) - # # assure that the robot did not move - # assert np.allclose(p1, p2) + class TestSimEnvsTquat(TestSimEnvs): @@ -270,32 +245,7 @@ def test_collision_tquat(self, cfg, gripper_cfg): _, _, _, _, info = env.step(collision_action) self.assert_collision(info) - # def test_collision_guard_tquat(self, cfg, gripper_cfg): - # """ - # Check that an obvious collision is detected by the CollisionGuard - # """ - # # env creation - # env = SimEnvCreator()( - # ControlMode.CARTESIAN_TQuat, - # cfg, - # gripper_cfg=gripper_cfg, - # collision_guard=True, - # cameras=None, - # max_relative_movement=None, - # ) - # obs, _ = env.reset() - # unwrapped = cast(RobotEnv, env.unwrapped) - # p1: np.ndarray = unwrapped.robot.get_joint_position() - # # an obvious below ground collision action - # obs["tquat"][0] = 0.4 - # obs["tquat"][2] = -0.05 - # collision_action = TQuatDictType(tquat=obs["tquat"]) - # collision_action.update(GripperDictType(gripper=np.array([0.0]))) - # _, _, _, _, info = env.step(collision_action) - # p2: np.ndarray = unwrapped.robot.get_joint_position() - # self.assert_collision(info) - # # assure that the robot did not move - # assert np.allclose(p1, p2) + class TestSimEnvsJoints(TestSimEnvs): @@ -359,31 +309,7 @@ def test_collision_joints(self, cfg, gripper_cfg): _, _, _, _, info = env.step(collision_act) self.assert_collision(info) - # def test_collision_guard_joints(self, cfg, gripper_cfg): - # """ - # Check that an obvious collision is detected by sim - # """ - # # env creation - # env = SimEnvCreator()( - # ControlMode.JOINTS, - # cfg, - # gripper_cfg=gripper_cfg, - # collision_guard=True, - # cameras=None, - # max_relative_movement=None, - # ) - # env.reset() - # unwrapped = cast(RobotEnv, env.unwrapped) - # p1: np.ndarray = unwrapped.robot.get_joint_position() - # # the below action is a test_case where there is an obvious collision regardless of the gripper action - # collision_act = JointsDictType(joints=np.array([0, 1.78, 0, -1.45, 0, 0, 0], dtype=np.float32)) - # collision_act.update(GripperDictType(gripper=np.array([1.0]))) - # _, _, _, _, info = env.step(collision_act) - # p2: np.ndarray = unwrapped.robot.get_joint_position() - - # self.assert_collision(info) - # # assure that the robot did not move - # assert np.allclose(p1, p2) + def test_relative_zero_action_joints(self, cfg, gripper_cfg): """ From 4352f1ce79272ee215ced02630d6922bbfb05c74 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Sat, 11 Apr 2026 12:07:44 +0200 Subject: [PATCH 5/7] fix(sim): CoverWrapper wrapper for correct resetting There is now a CoverWrapper which needs to be added at the top of the stack. It ensures that the sim is correctly reset before the wrappers assume a clean sim state. Its only strictly necessary for simulator environments but also works for hardware environments. --- README.md | 2 + examples/fr3/fr3_readme.py | 2 + extensions/rcs_fr3/src/rcs_fr3/creators.py | 3 ++ .../rcs_panda/src/rcs_panda/creators.py | 3 ++ .../rcs_so101/src/rcs_so101/creators.py | 2 + extensions/rcs_ur5e/src/rcs_ur5e/creators.py | 2 + .../rcs_xarm7/src/rcs_xarm7/creators.py | 2 + python/rcs/envs/base.py | 52 ++++++++++++------- python/rcs/envs/creators.py | 3 ++ python/rcs/rpc/server.py | 2 +- 10 files changed, 52 insertions(+), 21 deletions(-) diff --git a/README.md b/README.md index eb20482e..19f47aef 100644 --- a/README.md +++ b/README.md @@ -52,6 +52,7 @@ from rcs.envs.base import ( RelativeTo, RobotWrapper, SimEnv, + CoverWrapper, ) from rcs.envs.sim import GripperWrapperSim, RobotSimWrapper from rcs.envs.utils import ( @@ -98,6 +99,7 @@ if __name__ == "__main__": # relative actions bounded by 10cm translation and 10 degree rotation env = RelativeActionSpace(env, max_mov=(0.1, np.deg2rad(10)), relative_to=RelativeTo.LAST_STEP) + env = CoverWrapper(env) env.get_wrapper_attr("sim").open_gui() # wait for gui to open diff --git a/examples/fr3/fr3_readme.py b/examples/fr3/fr3_readme.py index fdf62c4e..5dafc344 100644 --- a/examples/fr3/fr3_readme.py +++ b/examples/fr3/fr3_readme.py @@ -11,6 +11,7 @@ RelativeTo, RobotWrapper, SimEnv, + CoverWrapper, ) from rcs.envs.sim import GripperWrapperSim, RobotSimWrapper from rcs.envs.utils import ( @@ -57,6 +58,7 @@ # relative actions bounded by 10cm translation and 10 degree rotation env = RelativeActionSpace(env, max_mov=(0.1, np.deg2rad(10)), relative_to=RelativeTo.LAST_STEP) + env = CoverWrapper(env) env.get_wrapper_attr("sim").open_gui() # wait for gui to open diff --git a/extensions/rcs_fr3/src/rcs_fr3/creators.py b/extensions/rcs_fr3/src/rcs_fr3/creators.py index ed970a78..3d4a6e47 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/creators.py +++ b/extensions/rcs_fr3/src/rcs_fr3/creators.py @@ -18,6 +18,7 @@ RelativeActionSpace, RelativeTo, RobotWrapper, + CoverWrapper, ) from rcs.envs.creators import RCSHardwareEnvCreator from rcs.hand.tilburg_hand import TilburgHand @@ -125,6 +126,7 @@ def __call__( # type: ignore # ) if relative_to != RelativeTo.NONE: env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) + env = CoverWrapper(env) return env @@ -173,6 +175,7 @@ def __call__( # type: ignore camera_set.wait_for_frames() logger.info("CameraSet started") env = CameraSetWrapper(env, camera_set) + env = CoverWrapper(env) return env diff --git a/extensions/rcs_panda/src/rcs_panda/creators.py b/extensions/rcs_panda/src/rcs_panda/creators.py index 37bcfe98..ebe2da1b 100644 --- a/extensions/rcs_panda/src/rcs_panda/creators.py +++ b/extensions/rcs_panda/src/rcs_panda/creators.py @@ -14,6 +14,7 @@ RelativeActionSpace, RelativeTo, RobotWrapper, + CoverWrapper, ) from rcs.envs.creators import RCSHardwareEnvCreator from rcs.hand.tilburg_hand import TilburgHand @@ -106,6 +107,7 @@ def __call__( # type: ignore # ) if max_relative_movement is not None: env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) + env = CoverWrapper(env) return env @@ -153,4 +155,5 @@ def __call__( # type: ignore camera_set.wait_for_frames() logger.info("CameraSet started") env = CameraSetWrapper(env, camera_set) + env = CoverWrapper(env) return env diff --git a/extensions/rcs_so101/src/rcs_so101/creators.py b/extensions/rcs_so101/src/rcs_so101/creators.py index 52054d36..112cdc5f 100644 --- a/extensions/rcs_so101/src/rcs_so101/creators.py +++ b/extensions/rcs_so101/src/rcs_so101/creators.py @@ -10,6 +10,7 @@ RelativeActionSpace, RelativeTo, RobotWrapper, + CoverWrapper, ) from rcs.envs.creators import RCSHardwareEnvCreator from rcs_so101 import SO101IK @@ -48,6 +49,7 @@ def __call__( # type: ignore if max_relative_movement is not None: env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) + env = CoverWrapper(env) return env diff --git a/extensions/rcs_ur5e/src/rcs_ur5e/creators.py b/extensions/rcs_ur5e/src/rcs_ur5e/creators.py index 6762b8d2..e640ee49 100644 --- a/extensions/rcs_ur5e/src/rcs_ur5e/creators.py +++ b/extensions/rcs_ur5e/src/rcs_ur5e/creators.py @@ -10,6 +10,7 @@ RelativeActionSpace, RelativeTo, RobotWrapper, + CoverWrapper, ) from rcs.envs.creators import RCSHardwareEnvCreator from rcs_ur5e.hw import RobotiQGripper, UR5e, UR5eConfig @@ -51,5 +52,6 @@ def __call__( # type: ignore if max_relative_movement is not None: env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) + env = CoverWrapper(env) return env diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py index efe4e5ad..f61c74e4 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py +++ b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py @@ -12,6 +12,7 @@ RelativeActionSpace, RelativeTo, RobotWrapper, + CoverWrapper, ) from rcs.envs.creators import RCSHardwareEnvCreator from rcs.hand.tilburg_hand import THConfig, TilburgHand @@ -49,5 +50,6 @@ def __call__( # type: ignore if max_relative_movement is not None: env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) + env = CoverWrapper(env) return env diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index 890934bb..e3dd1410 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -244,8 +244,7 @@ def step_sim(self): else: self.sim.step_until_convergence() - def reset_sim(self): - self.sim.reset() + def apply_sim_state(self): self.sim.step(1) @@ -255,7 +254,20 @@ def reset( if self.main_greenlet is not None: self.main_greenlet.switch() else: - self.reset_sim() + self.apply_sim_state() + return super().reset(seed=seed, options=options) + +class CoverWrapper(gym.Wrapper): + """The CoverWrapper must be the last wrapper on the stack + + Only strictly necessary for simulator environments, but also works for hardware environments. + It takes care of resetting the simulator before any other wrapper resets its state, already assuming + a fresh simulator state. + """ + def reset(self, *, seed: int | None = None, options: dict[str, Any] | None = None) -> tuple[dict[str, Any], dict[str, Any]]: + if self.env.get_wrapper_attr("PLATFORM") == RobotPlatform.SIMULATION: + sim = cast(simulation.Sim, self.get_wrapper_attr("sim")) + sim.reset() return super().reset(seed=seed, options=options) @@ -333,25 +345,28 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: ): msg = "Given type is not matching control mode!" raise RuntimeError(msg) + last_action = self.prev_action self.prev_action = copy.deepcopy(action) + # shallow copy + action = dict(action) if self.get_base_control_mode() == ControlMode.JOINTS and ( - self.prev_action is None - or not np.allclose(action[self.joints_key], self.prev_action[self.joints_key], atol=1e-03, rtol=0) + last_action is None + or not np.allclose(action[self.joints_key], last_action[self.joints_key], atol=1e-03, rtol=0) ): self.robot.set_joint_position(action[self.joints_key]) action.pop(self.joints_key) elif self.get_base_control_mode() == ControlMode.CARTESIAN_TRPY and ( - self.prev_action is None - or not np.allclose(action[self.trpy_key], self.prev_action[self.trpy_key], atol=1e-03, rtol=0) + last_action is None + or not np.allclose(action[self.trpy_key], last_action[self.trpy_key], atol=1e-03, rtol=0) ): self.robot.set_cartesian_position( common.Pose(translation=action[self.trpy_key][:3], rpy_vector=action[self.trpy_key][3:]) ) action.pop(self.trpy_key) elif self.get_base_control_mode() == ControlMode.CARTESIAN_TQuat and ( - self.prev_action is None - or not np.allclose(action[self.tquat_key], self.prev_action[self.tquat_key], atol=1e-03, rtol=0) + last_action is None + or not np.allclose(action[self.tquat_key], last_action[self.tquat_key], atol=1e-03, rtol=0) ): self.robot.set_cartesian_position( common.Pose(translation=action[self.tquat_key][:3], quaternion=action[self.tquat_key][3:]) @@ -361,18 +376,13 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: def observation(self, observation: dict, info: dict[str, Any]) -> tuple[dict[str, Any], dict[str, Any]]: observation.update(self.get_robot_obs()) - # if self.env.get_wrapper_attr("PLATFORM") == RobotPlatform.SIMULATION: - # sim_robot = cast(SimRobot, self.robot) - # state = sim_robot.get_state() - # info["collision"] = state.collision - # info["ik_success"] = state.ik_success - # info["is_sim_converged"] = self.env.get_wrapper_attr("sim").is_converged() return observation, info def reset( self, *, seed: int | None = None, options: dict[str, Any] | None = None ) -> tuple[dict[str, Any], dict[str, Any]]: + self.prev_action = None self.robot.reset() if self.home_on_reset: self.robot.move_home() @@ -405,6 +415,7 @@ def __init__( else: self.robot2world = robot2world self.lead_env: gym.Env | None = None + self.sim: simulation.Sim | None = None # make sure all envs are the same type (sim/real) for env in self.envs: @@ -416,6 +427,9 @@ def __init__( self._runs_in_sim = self.PLATFORM == RobotPlatform.SIMULATION if self._runs_in_sim: self._inject_main_greenlet() + assert isinstance(self.lead_env, SimEnv), "something is wrong with the env, the base should be type SimEnv" + self.sim = self.lead_env.get_wrapper_attr("sim") + def _inject_main_greenlet(self): main_gr = getcurrent() @@ -471,9 +485,7 @@ def make_step_gr(env_to_step): if self._runs_in_sim: # SIM path: 3. UP: Gather observations # Resume robot greenlet. It returns the step results. - res = step_greenlets[key].switch() - ob, r, t, tr, i = res - + ob, r, t, tr, info[key] = step_greenlets[key].switch() else: # HARDWARE path act = self._translate_pose(key, action[key], to_world=False) @@ -510,9 +522,9 @@ def make_reset_gr(env_to_reset, s, o): reset_greenlets[key] = gr gr.switch() - # SIM path: 2. SIM: reset + # SIM path: 2. SIM: apply state from rested wrappers assert isinstance(self.lead_env, SimEnv) - self.lead_env.reset_sim() + self.lead_env.apply_sim_state() for key, env in self.envs.items(): diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index 69e7fcf9..f7ad2e1e 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -20,6 +20,7 @@ RelativeTo, RobotWrapper, SimEnv, + CoverWrapper, ) from rcs.envs.sim import ( GripperWrapperSim, @@ -128,6 +129,7 @@ def __call__( # type: ignore # ) if max_relative_movement is not None: env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) + env = CoverWrapper(env) return env @@ -186,6 +188,7 @@ def __call__( # type: ignore BaseCameraSet, SimCameraSet(simulation, cameras, physical_units=True, render_on_demand=True) ) env = CameraSetWrapper(env, camera_set, include_depth=True) + env = CoverWrapper(env) return env diff --git a/python/rcs/rpc/server.py b/python/rcs/rpc/server.py index 029aca47..78030985 100644 --- a/python/rcs/rpc/server.py +++ b/python/rcs/rpc/server.py @@ -26,7 +26,7 @@ def reset(self, **kwargs): @rpyc.exposed def get_robot_obs(self): """Get the current observation using the Wrapper base class if available.""" - return self.get_wrapper_attr("get_robot_obs")() + return self.env.get_wrapper_attr("get_robot_obs")() @rpyc.exposed def unwrapped(self): From 9af83f64eedb89c189dc60e7831d9360244a5f78 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Sat, 11 Apr 2026 12:09:08 +0200 Subject: [PATCH 6/7] style: python formatting --- README.md | 2 +- examples/fr3/fr3_readme.py | 2 +- extensions/rcs_fr3/src/rcs_fr3/creators.py | 2 +- .../rcs_panda/src/rcs_panda/creators.py | 2 +- .../rcs_so101/src/rcs_so101/creators.py | 2 +- extensions/rcs_ur5e/src/rcs_ur5e/creators.py | 2 +- .../rcs_xarm7/src/rcs_xarm7/creators.py | 2 +- python/rcs/envs/base.py | 38 ++--- python/rcs/envs/creators.py | 2 +- python/rcs/envs/sim.py | 156 +++--------------- python/rcs/ompl/mj_ompl.py | 2 +- python/tests/test_sim_envs.py | 6 - src/pybind/rcs.cpp | 9 +- 13 files changed, 59 insertions(+), 168 deletions(-) diff --git a/README.md b/README.md index 19f47aef..a9aa7779 100644 --- a/README.md +++ b/README.md @@ -47,12 +47,12 @@ from rcs.camera.sim import SimCameraSet from rcs.envs.base import ( CameraSetWrapper, ControlMode, + CoverWrapper, GripperWrapper, RelativeActionSpace, RelativeTo, RobotWrapper, SimEnv, - CoverWrapper, ) from rcs.envs.sim import GripperWrapperSim, RobotSimWrapper from rcs.envs.utils import ( diff --git a/examples/fr3/fr3_readme.py b/examples/fr3/fr3_readme.py index 5dafc344..1ca2948f 100644 --- a/examples/fr3/fr3_readme.py +++ b/examples/fr3/fr3_readme.py @@ -6,12 +6,12 @@ from rcs.envs.base import ( CameraSetWrapper, ControlMode, + CoverWrapper, GripperWrapper, RelativeActionSpace, RelativeTo, RobotWrapper, SimEnv, - CoverWrapper, ) from rcs.envs.sim import GripperWrapperSim, RobotSimWrapper from rcs.envs.utils import ( diff --git a/extensions/rcs_fr3/src/rcs_fr3/creators.py b/extensions/rcs_fr3/src/rcs_fr3/creators.py index 3d4a6e47..6a503e86 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/creators.py +++ b/extensions/rcs_fr3/src/rcs_fr3/creators.py @@ -11,6 +11,7 @@ from rcs.envs.base import ( CameraSetWrapper, ControlMode, + CoverWrapper, GripperWrapper, HandWrapper, HardwareEnv, @@ -18,7 +19,6 @@ RelativeActionSpace, RelativeTo, RobotWrapper, - CoverWrapper, ) from rcs.envs.creators import RCSHardwareEnvCreator from rcs.hand.tilburg_hand import TilburgHand diff --git a/extensions/rcs_panda/src/rcs_panda/creators.py b/extensions/rcs_panda/src/rcs_panda/creators.py index ebe2da1b..af1bfd15 100644 --- a/extensions/rcs_panda/src/rcs_panda/creators.py +++ b/extensions/rcs_panda/src/rcs_panda/creators.py @@ -7,6 +7,7 @@ from rcs.envs.base import ( CameraSetWrapper, ControlMode, + CoverWrapper, GripperWrapper, HandWrapper, HardwareEnv, @@ -14,7 +15,6 @@ RelativeActionSpace, RelativeTo, RobotWrapper, - CoverWrapper, ) from rcs.envs.creators import RCSHardwareEnvCreator from rcs.hand.tilburg_hand import TilburgHand diff --git a/extensions/rcs_so101/src/rcs_so101/creators.py b/extensions/rcs_so101/src/rcs_so101/creators.py index 112cdc5f..035dd8bb 100644 --- a/extensions/rcs_so101/src/rcs_so101/creators.py +++ b/extensions/rcs_so101/src/rcs_so101/creators.py @@ -5,12 +5,12 @@ from rcs.envs.base import ( CameraSetWrapper, ControlMode, + CoverWrapper, GripperWrapper, HardwareEnv, RelativeActionSpace, RelativeTo, RobotWrapper, - CoverWrapper, ) from rcs.envs.creators import RCSHardwareEnvCreator from rcs_so101 import SO101IK diff --git a/extensions/rcs_ur5e/src/rcs_ur5e/creators.py b/extensions/rcs_ur5e/src/rcs_ur5e/creators.py index e640ee49..048a833b 100644 --- a/extensions/rcs_ur5e/src/rcs_ur5e/creators.py +++ b/extensions/rcs_ur5e/src/rcs_ur5e/creators.py @@ -5,12 +5,12 @@ from rcs.envs.base import ( CameraSetWrapper, ControlMode, + CoverWrapper, GripperWrapper, HardwareEnv, RelativeActionSpace, RelativeTo, RobotWrapper, - CoverWrapper, ) from rcs.envs.creators import RCSHardwareEnvCreator from rcs_ur5e.hw import RobotiQGripper, UR5e, UR5eConfig diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py index f61c74e4..9b84c195 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py +++ b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py @@ -7,12 +7,12 @@ from rcs.envs.base import ( CameraSetWrapper, ControlMode, + CoverWrapper, HandWrapper, HardwareEnv, RelativeActionSpace, RelativeTo, RobotWrapper, - CoverWrapper, ) from rcs.envs.creators import RCSHardwareEnvCreator from rcs.hand.tilburg_hand import THConfig, TilburgHand diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index e3dd1410..9cb3ace2 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -5,9 +5,9 @@ from enum import Enum, auto from typing import Annotated, Any, ClassVar, Literal, TypeAlias, cast -from greenlet import getcurrent, greenlet import gymnasium as gym import numpy as np +from greenlet import getcurrent, greenlet from rcs._core.common import Hand, RobotPlatform from rcs._core.sim import SimRobot from rcs.camera.interface import BaseCameraSet @@ -22,10 +22,10 @@ get_space, get_space_keys, ) +from rcs.utils import SimpleFrameRate from rcs import common from rcs import sim as simulation -from rcs.utils import SimpleFrameRate _logger = logging.getLogger(__name__) @@ -200,6 +200,7 @@ def get_home_position(robot: common.Robot) -> np.ndarray: """Returns the home position of the robot.""" return common.robots_meta_config(robot.get_config().robot_type).q_home + class BaseEnv(gym.Env): PLATFORM: RobotPlatform @@ -213,7 +214,6 @@ def reset( return {}, {} - class HardwareEnv(BaseEnv): PLATFORM = RobotPlatform.HARDWARE @@ -247,7 +247,6 @@ def step_sim(self): def apply_sim_state(self): self.sim.step(1) - def reset( self, *, seed: int | None = None, options: dict[str, Any] | None = None ) -> tuple[dict[str, Any], dict[str, Any]]: @@ -257,22 +256,24 @@ def reset( self.apply_sim_state() return super().reset(seed=seed, options=options) + class CoverWrapper(gym.Wrapper): """The CoverWrapper must be the last wrapper on the stack - + Only strictly necessary for simulator environments, but also works for hardware environments. It takes care of resetting the simulator before any other wrapper resets its state, already assuming a fresh simulator state. """ - def reset(self, *, seed: int | None = None, options: dict[str, Any] | None = None) -> tuple[dict[str, Any], dict[str, Any]]: + + def reset( + self, *, seed: int | None = None, options: dict[str, Any] | None = None + ) -> tuple[dict[str, Any], dict[str, Any]]: if self.env.get_wrapper_attr("PLATFORM") == RobotPlatform.SIMULATION: sim = cast(simulation.Sim, self.get_wrapper_attr("sim")) sim.reset() return super().reset(seed=seed, options=options) - - class RobotWrapper(ActObsInfoWrapper): """Gym Wrapper for a single robot arm. @@ -333,7 +334,6 @@ def get_robot_obs(self) -> ArmObsType: xyzrpy=self.robot.get_cartesian_position().xyzrpy(), ) - def action(self, action: dict[str, Any]) -> dict[str, Any]: if ( self.get_base_control_mode() == ControlMode.CARTESIAN_TQuat @@ -378,7 +378,6 @@ def observation(self, observation: dict, info: dict[str, Any]) -> tuple[dict[str observation.update(self.get_robot_obs()) return observation, info - def reset( self, *, seed: int | None = None, options: dict[str, Any] | None = None ) -> tuple[dict[str, Any], dict[str, Any]]: @@ -396,7 +395,7 @@ def close(self): class MultiRobotWrapper(gym.Env): """Wraps a dictionary of single robot environments to allow for multi robot control. - Env1 Env2 Env3 + Env1 Env2 Env3 | | | ---------------- MultiRobotWrapper @@ -423,18 +422,21 @@ def __init__( self.PLATFORM = self.envs[env].get_wrapper_attr("PLATFORM") self.lead_env = self.envs[env].unwrapped else: - assert self.envs[env].get_wrapper_attr("PLATFORM") == self.PLATFORM, "all envs must have the same platform!" + assert ( + self.envs[env].get_wrapper_attr("PLATFORM") == self.PLATFORM + ), "all envs must have the same platform!" self._runs_in_sim = self.PLATFORM == RobotPlatform.SIMULATION if self._runs_in_sim: self._inject_main_greenlet() assert isinstance(self.lead_env, SimEnv), "something is wrong with the env, the base should be type SimEnv" self.sim = self.lead_env.get_wrapper_attr("sim") - def _inject_main_greenlet(self): main_gr = getcurrent() for env_item in self.envs.values(): - assert isinstance(env_item.unwrapped, SimEnv), "something is wrong with the env, the base should be type SimEnv" + assert isinstance( + env_item.unwrapped, SimEnv + ), "something is wrong with the env, the base should be type SimEnv" env_item.unwrapped.main_greenlet = main_gr def _translate_pose(self, key, dic, to_world=True): @@ -472,7 +474,6 @@ def make_step_gr(env_to_step): assert isinstance(self.lead_env, SimEnv) self.lead_env.step_sim() - # follows gym env by combinding a dict of envs into a single env obs = {} reward = 0.0 @@ -481,7 +482,6 @@ def make_step_gr(env_to_step): info = {} for key, env in self.envs.items(): - if self._runs_in_sim: # SIM path: 3. UP: Gather observations # Resume robot greenlet. It returns the step results. @@ -491,8 +491,6 @@ def make_step_gr(env_to_step): act = self._translate_pose(key, action[key], to_world=False) ob, r, t, tr, info[key] = env.step(act) - - obs[key] = self._translate_pose(key, ob, to_world=True) reward += float(r) terminated = terminated or t @@ -510,11 +508,11 @@ def reset( seed_ = {key: seed for key in self.envs} if seed is not None else {key: None for key in self.envs} options_ = options if options is not None else {key: None for key in self.envs} - reset_greenlets = {} if self._runs_in_sim: # SIM path: 1. DOWN: Reset each robot for key, env in self.envs.items(): + def make_reset_gr(env_to_reset, s, o): return greenlet(lambda: env_to_reset.reset(seed=s, options=o)) @@ -526,7 +524,6 @@ def make_reset_gr(env_to_reset, s, o): assert isinstance(self.lead_env, SimEnv) self.lead_env.apply_sim_state() - for key, env in self.envs.items(): if self._runs_in_sim: # SIM path: 3. UP: Gather initial obs @@ -538,7 +535,6 @@ def make_reset_gr(env_to_reset, s, o): obs[key] = self._translate_pose(key, ob, to_world=True) info[key] = i - return obs, info def get_wrapper_attr(self, name: str) -> Any: diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index f7ad2e1e..1a45ff69 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -13,6 +13,7 @@ from rcs.envs.base import ( CameraSetWrapper, ControlMode, + CoverWrapper, GripperWrapper, HandWrapper, MultiRobotWrapper, @@ -20,7 +21,6 @@ RelativeTo, RobotWrapper, SimEnv, - CoverWrapper, ) from rcs.envs.sim import ( GripperWrapperSim, diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index e78dab30..8b3ac8cf 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -4,9 +4,7 @@ import gymnasium as gym import numpy as np from rcs._core.common import RobotPlatform -from rcs.envs.base import ( - GripperWrapper, -) +from rcs.envs.base import GripperWrapper from rcs.envs.space_utils import ActObsInfoWrapper import rcs @@ -28,7 +26,6 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: self.sim_robot.clear_collision_flag() return action - def observation(self, observation: dict, info: dict[str, Any]) -> tuple[dict[str, Any], dict[str, Any]]: state = self.sim_robot.get_state() if "collision" not in info: @@ -39,21 +36,6 @@ def observation(self, observation: dict, info: dict[str, Any]) -> tuple[dict[str info["is_sim_converged"] = self.sim.is_converged() return observation, info - - # def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, bool, dict]: - # self.sim_robot.clear_collision_flag() - # obs, _, _, _, info = super().step(action) - - # state = self.sim_robot.get_state() - # if "collision" not in info: - # info["collision"] = state.collision - # else: - # info["collision"] = info["collision"] or state.collision - # info["ik_success"] = state.ik_success - # info["is_sim_converged"] = self.sim.is_converged() - # # truncate episode if collision - # return obs, 0, False, info["collision"] or not state.ik_success, info - def reset( self, *, seed: int | None = None, options: dict[str, Any] | None = None ) -> tuple[dict[str, Any], dict[str, Any]]: @@ -61,111 +43,13 @@ def reset( return super().reset(seed=seed, options=options) -# class MultiSimRobotWrapper(gym.Wrapper): -# """Wraps a dictionary of environments to allow for multi robot control.""" - -# def __init__(self, env: MultiRobotWrapper, simulation: sim.Sim): -# super().__init__(env) -# self.env: MultiRobotWrapper -# self.sim = simulation -# self.sim_robots = cast(dict[str, sim.SimRobot], {key: e.robot for key, e in self.env.unwrapped_multi.items()}) -# self._inject_main_greenlet() - -# def _inject_main_greenlet(self): -# main_gr = getcurrent() -# for env_item in self.env.envs.values(): -# curr = env_item -# while True: -# if isinstance(curr, RobotSimWrapper): -# curr.main_greenlet = main_gr -# break -# if not hasattr(curr, "env"): -# break -# curr = curr.env - -# def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, bool, dict]: -# # 1. DOWN: Set actions for all robots -# step_greenlets = {} -# for key, env in self.env.envs.items(): - -# def make_step_gr(env_to_step): -# return greenlet(env_to_step.step) - -# gr = make_step_gr(env) -# step_greenlets[key] = gr - -# # Translate action -# act = self.env._translate_pose(key, action[key], to_world=False) - -# # Switch to robot greenlet. It will run until RobotSimWrapper.step switches back. -# gr.switch(act) - -# # 2. SIM: Step physics once -# self.sim.step_until_convergence() - -# # 3. UP: Gather observations -# obs = {} -# reward = 0.0 -# terminated = False -# truncated = False -# info = {} - -# for key in self.env.envs: -# # Resume robot greenlet. It returns the step results. -# res = step_greenlets[key].switch() -# ob, r, t, tr, i = res - -# # Translate observation back to world coordinates -# obs[key] = self.env._translate_pose(key, ob, to_world=True) -# reward += float(r) -# terminated = terminated or t -# truncated = truncated or tr -# info[key] = i -# info[key]["terminated"] = t -# info[key]["truncated"] = tr -# info[key]["is_sim_converged"] = self.sim.is_converged() - -# return obs, reward, terminated, truncated, info - -# def reset( # type: ignore -# self, *, seed: dict[str, int | None] | None = None, options: dict[str, Any] | None = None -# ) -> tuple[dict[str, Any], dict[str, Any]]: -# if seed is None: -# seed = dict.fromkeys(self.env.envs) -# if options is None: -# options = {key: {} for key in self.env.envs} -# obs = {} -# info = {} -# self.sim.reset() - -# # 1. DOWN: Reset each robot -# reset_greenlets = {} -# for key, env in self.env.envs.items(): - -# def make_reset_gr(env_to_reset, s, o): -# return greenlet(lambda: env_to_reset.reset(seed=s, options=o)) - -# gr = make_reset_gr(env, seed[key], options[key]) -# reset_greenlets[key] = gr -# gr.switch() - -# # 2. SIM: Initial step -# self.sim.step(1) - -# # 3. UP: Gather initial obs -# for key in self.env.envs: -# ob, i = reset_greenlets[key].switch() -# obs[key] = self.env._translate_pose(key, ob, to_world=True) -# info[key] = i - -# return obs, info - - class GripperWrapperSim(ActObsInfoWrapper): def __init__(self, env): super().__init__(env) assert self.env.get_wrapper_attr("PLATFORM") == RobotPlatform.SIMULATION, "Base environment must be simulation." - assert isinstance(self.get_wrapper_attr("gripper"), sim.SimGripper), "Gripper must be a sim.SimGripper instance." + assert isinstance( + self.get_wrapper_attr("gripper"), sim.SimGripper + ), "Gripper must be a sim.SimGripper instance." self._gripper = cast(sim.SimGripper, self.get_wrapper_attr("gripper")) def action(self, action: dict[str, Any]) -> dict[str, Any]: @@ -191,7 +75,9 @@ class HandWrapperSim(ActObsInfoWrapper): def __init__(self, env): super().__init__(env) assert self.env.get_wrapper_attr("PLATFORM") == RobotPlatform.SIMULATION, "Base environment must be simulation." - assert isinstance(self.get_wrapper_attr("hand"), sim.SimTilburgHand), "Hand must be a sim.SimTilburgHand instance." + assert isinstance( + self.get_wrapper_attr("hand"), sim.SimTilburgHand + ), "Hand must be a sim.SimTilburgHand instance." self._hand = cast(sim.SimTilburgHand, self.get_wrapper_attr("hand")) def action(self, action: dict[str, Any]) -> dict[str, Any]: @@ -211,8 +97,6 @@ def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tupl return observation, info - - class RandomObjectPos(gym.Wrapper): """ Wrapper to randomly re-place an object in the lab environments. @@ -283,14 +167,22 @@ def reset( quat[2] + random_z_rotation, ] else: - self.get_wrapper_attr("sim").data.joint(self.joint_name).qpos = [pos_x, pos_y, pos_z, quat[3], quat[0], quat[1], quat[2]] + self.get_wrapper_attr("sim").data.joint(self.joint_name).qpos = [ + pos_x, + pos_y, + pos_z, + quat[3], + quat[0], + quat[1], + quat[2], + ] return obs, info class RandomCubePos(gym.Wrapper): """Wrapper to randomly place cube in the lab environments. - + Works only for single robot """ @@ -312,7 +204,15 @@ def reset( pos_y = iso_cube[1] + np.random.random() * 0.2 - 0.1 if self.include_rotation: - self.get_wrapper_attr("sim").data.joint(self.cube_joint_name).qpos = [pos_x, pos_y, pos_z, 2 * np.random.random() - 1, 0, 0, 1] + self.get_wrapper_attr("sim").data.joint(self.cube_joint_name).qpos = [ + pos_x, + pos_y, + pos_z, + 2 * np.random.random() - 1, + 0, + 0, + 1, + ] else: self.get_wrapper_attr("sim").data.joint(self.cube_joint_name).qpos = [pos_x, pos_y, pos_z, 0, 0, 0, 1] @@ -351,9 +251,7 @@ def step(self, action: dict[str, Any]): # type: ignore self._gripper_closing = 0 cube_pose = rcs.common.Pose(translation=self.sim.data.geom(self.cube_geom_name).xpos) cube_pose = self._robot.to_pose_in_robot_coordinates(cube_pose) - tcp_to_obj_dist = np.linalg.norm( - cube_pose.translation() - self._robot.get_cartesian_position().translation() - ) + tcp_to_obj_dist = np.linalg.norm(cube_pose.translation() - self._robot.get_cartesian_position().translation()) obj_to_goal_dist = 0.10 - min(cube_pose.translation()[-1], 0.10) obj_to_goal_dist = np.linalg.norm(cube_pose.translation() - self.home_pose.translation()) # NOTE: 4 depends on the time passing between each step. diff --git a/python/rcs/ompl/mj_ompl.py b/python/rcs/ompl/mj_ompl.py index 6d865cea..4972ed9e 100644 --- a/python/rcs/ompl/mj_ompl.py +++ b/python/rcs/ompl/mj_ompl.py @@ -2,12 +2,12 @@ import xml.etree.ElementTree as ET from copy import deepcopy +import gymnasium as gym import mujoco as mj import numpy as np from ompl import base as ob from ompl import geometric as og from rcs._core.common import Pose -import gymnasium as gym DEFAULT_PLANNING_TIME = 5.0 # Default time allowed for planning in seconds INTERPOLATE_NUM = 500 # Number of points to interpolate between start and goal states diff --git a/python/tests/test_sim_envs.py b/python/tests/test_sim_envs.py index d00eb49c..2da01ab1 100644 --- a/python/tests/test_sim_envs.py +++ b/python/tests/test_sim_envs.py @@ -151,8 +151,6 @@ def test_collision_trpy(self, cfg, gripper_cfg): self.assert_collision(info) - - class TestSimEnvsTquat(TestSimEnvs): """This class is for testing Tquat sim env functionalities""" @@ -246,8 +244,6 @@ def test_collision_tquat(self, cfg, gripper_cfg): self.assert_collision(info) - - class TestSimEnvsJoints(TestSimEnvs): """This class is for testing Joints sim env functionalities""" @@ -309,8 +305,6 @@ def test_collision_joints(self, cfg, gripper_cfg): _, _, _, _, info = env.step(collision_act) self.assert_collision(info) - - def test_relative_zero_action_joints(self, cfg, gripper_cfg): """ Check that an obvious collision is detected by the CollisionGuard diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 8cc5f916..0177580f 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -482,7 +482,8 @@ PYBIND11_MODULE(_core, m) { [](const rcs::sim::SimRobotConfig& self, py::dict) { return rcs::sim::SimRobotConfig(self); }) - .def("add_postfix", &rcs::sim::SimRobotConfig::add_postfix, py::arg("id")); + .def("add_postfix", &rcs::sim::SimRobotConfig::add_postfix, + py::arg("id")); py::class_(sim, "SimRobotState") .def(py::init<>()) @@ -528,7 +529,8 @@ PYBIND11_MODULE(_core, m) { [](const rcs::sim::SimGripperConfig& self, py::dict) { return rcs::sim::SimGripperConfig(self); }) - .def("add_postfix", &rcs::sim::SimGripperConfig::add_postfix, py::arg("id")); + .def("add_postfix", &rcs::sim::SimGripperConfig::add_postfix, + py::arg("id")); py::class_( sim, "SimGripperState") .def(py::init<>()) @@ -612,7 +614,8 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("grasp_type", &rcs::sim::SimTilburgHandConfig::grasp_type) .def_readwrite("seconds_between_callbacks", &rcs::sim::SimTilburgHandConfig::seconds_between_callbacks) - .def("add_postfix", &rcs::sim::SimTilburgHandConfig::add_postfix, py::arg("id")); + .def("add_postfix", &rcs::sim::SimTilburgHandConfig::add_postfix, + py::arg("id")); // SimTilburgHand py::class_>(sim, "SimTilburgHand") From 7dc46b23cf9604c200482e70bf2999e7398fe777 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Sat, 11 Apr 2026 13:45:13 +0200 Subject: [PATCH 7/7] lint: fix python linting errors --- README.md | 3 ++- examples/fr3/fr3_readme.py | 3 ++- extensions/rcs_fr3/src/rcs_fr3/creators.py | 12 +++++------- extensions/rcs_panda/src/rcs_panda/creators.py | 12 +++++------- extensions/rcs_so101/src/rcs_so101/creators.py | 6 ++---- extensions/rcs_ur5e/src/rcs_ur5e/creators.py | 6 ++---- extensions/rcs_xarm7/src/rcs_xarm7/creators.py | 6 ++---- python/rcs/envs/base.py | 5 +++-- python/rcs/envs/creators.py | 15 ++++++--------- python/rcs/sim/egl_bootstrap.py | 7 ++++--- python/tests/test_multi_sim.py | 6 +++--- 11 files changed, 36 insertions(+), 45 deletions(-) diff --git a/README.md b/README.md index a9aa7779..ad374772 100644 --- a/README.md +++ b/README.md @@ -41,6 +41,7 @@ Flexibly compose your Gymnasium environment to fit your exact training needs. *F ```python from time import sleep +import gymnasium as gym import numpy as np from rcs._core.sim import SimConfig from rcs.camera.sim import SimCameraSet @@ -83,7 +84,7 @@ if __name__ == "__main__": # base env robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) - env = SimEnv(simulation) + env: gym.Env = SimEnv(simulation) env = RobotWrapper(env, robot, ControlMode.CARTESIAN_TQuat) # gripper diff --git a/examples/fr3/fr3_readme.py b/examples/fr3/fr3_readme.py index 1ca2948f..6e2dd5f3 100644 --- a/examples/fr3/fr3_readme.py +++ b/examples/fr3/fr3_readme.py @@ -1,5 +1,6 @@ from time import sleep +import gymnasium as gym import numpy as np from rcs._core.sim import SimConfig from rcs.camera.sim import SimCameraSet @@ -42,7 +43,7 @@ # base env robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) - env = SimEnv(simulation) + env: gym.Env = SimEnv(simulation) env = RobotWrapper(env, robot, ControlMode.CARTESIAN_TQuat) # gripper diff --git a/extensions/rcs_fr3/src/rcs_fr3/creators.py b/extensions/rcs_fr3/src/rcs_fr3/creators.py index 6a503e86..569cf483 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/creators.py +++ b/extensions/rcs_fr3/src/rcs_fr3/creators.py @@ -93,7 +93,7 @@ def __call__( # type: ignore robot = hw.Franka(ip, ik) robot.set_config(robot_cfg) - env = HardwareEnv() + env: gym.Env = HardwareEnv() env = RobotWrapper(env, robot, ControlMode.JOINTS if collision_guard is not None else control_mode) env = FR3HW(env) @@ -126,9 +126,7 @@ def __call__( # type: ignore # ) if relative_to != RelativeTo.NONE: env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) - env = CoverWrapper(env) - - return env + return CoverWrapper(env) class RCSFR3MultiEnvCreator(RCSHardwareEnvCreator): @@ -156,7 +154,8 @@ def __call__( # type: ignore robots[key] = hw.Franka(ip, ik) robots[key].set_config(robot_cfg) - envs = {} + envs: dict[str, gym.Env] = {} + env: gym.Env for key, ip in name2ip.items(): env = HardwareEnv() env = RobotWrapper(env, robots[key], control_mode) @@ -175,8 +174,7 @@ def __call__( # type: ignore camera_set.wait_for_frames() logger.info("CameraSet started") env = CameraSetWrapper(env, camera_set) - env = CoverWrapper(env) - return env + return CoverWrapper(env) class RCSFR3DefaultEnvCreator(RCSHardwareEnvCreator): diff --git a/extensions/rcs_panda/src/rcs_panda/creators.py b/extensions/rcs_panda/src/rcs_panda/creators.py index af1bfd15..b5d452ee 100644 --- a/extensions/rcs_panda/src/rcs_panda/creators.py +++ b/extensions/rcs_panda/src/rcs_panda/creators.py @@ -70,7 +70,7 @@ def __call__( # type: ignore robot = hw.Franka(ip, ik) robot.set_config(robot_cfg) - env = HardwareEnv() + env: gym.Env = HardwareEnv() env = RobotWrapper( env, robot, @@ -107,9 +107,7 @@ def __call__( # type: ignore # ) if max_relative_movement is not None: env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) - env = CoverWrapper(env) - - return env + return CoverWrapper(env) class RCSPandaMultiEnvCreator(RCSHardwareEnvCreator): @@ -136,7 +134,8 @@ def __call__( # type: ignore robots[ip] = hw.Franka(ip, ik) robots[ip].set_config(robot_cfg) - envs = {} + envs: dict[str, gym.Env] = {} + env: gym.Env for ip in ips: env = HardwareEnv() env = RobotWrapper(env, robots[ip], control_mode) @@ -155,5 +154,4 @@ def __call__( # type: ignore camera_set.wait_for_frames() logger.info("CameraSet started") env = CameraSetWrapper(env, camera_set) - env = CoverWrapper(env) - return env + return CoverWrapper(env) diff --git a/extensions/rcs_so101/src/rcs_so101/creators.py b/extensions/rcs_so101/src/rcs_so101/creators.py index 035dd8bb..6c541bec 100644 --- a/extensions/rcs_so101/src/rcs_so101/creators.py +++ b/extensions/rcs_so101/src/rcs_so101/creators.py @@ -35,7 +35,7 @@ def __call__( # type: ignore urdf=robot_cfg.kinematic_model_path.endswith(".urdf"), ) robot = SO101(robot_cfg=robot_cfg, ik=ik) - env = HardwareEnv() + env: gym.Env = HardwareEnv() env = RobotWrapper(env, robot, control_mode, home_on_reset=True) gripper = SO101Gripper(robot._hf_robot, robot) @@ -49,9 +49,7 @@ def __call__( # type: ignore if max_relative_movement is not None: env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) - env = CoverWrapper(env) - - return env + return CoverWrapper(env) # For now, the leader-follower teleop script uses the leader object directly # and doesn't depend on an RCS-provided class. diff --git a/extensions/rcs_ur5e/src/rcs_ur5e/creators.py b/extensions/rcs_ur5e/src/rcs_ur5e/creators.py index 048a833b..3cc6e2c0 100644 --- a/extensions/rcs_ur5e/src/rcs_ur5e/creators.py +++ b/extensions/rcs_ur5e/src/rcs_ur5e/creators.py @@ -38,7 +38,7 @@ def __call__( # type: ignore ) robot = UR5e(ip, ik) robot.set_config(robot_cfg) - env = HardwareEnv() + env: gym.Env = HardwareEnv() env = RobotWrapper(env, robot, control_mode, home_on_reset=True) gripper = RobotiQGripper(ip) @@ -52,6 +52,4 @@ def __call__( # type: ignore if max_relative_movement is not None: env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) - env = CoverWrapper(env) - - return env + return CoverWrapper(env) diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py index 9b84c195..9a97b3b9 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py +++ b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py @@ -36,7 +36,7 @@ def __call__( # type: ignore if isinstance(calibration_dir, str): calibration_dir = Path(calibration_dir) robot = XArm7(ip=ip) - env = HardwareEnv() + env: gym.Env = HardwareEnv() env = RobotWrapper(env, robot, control_mode, home_on_reset=True) if camera_set is not None: @@ -50,6 +50,4 @@ def __call__( # type: ignore if max_relative_movement is not None: env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) - env = CoverWrapper(env) - - return env + return CoverWrapper(env) diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index 9cb3ace2..fb6cf410 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -9,7 +9,6 @@ import numpy as np from greenlet import getcurrent, greenlet from rcs._core.common import Hand, RobotPlatform -from rcs._core.sim import SimRobot from rcs.camera.interface import BaseCameraSet from rcs.envs.space_utils import ( ActObsInfoWrapper, @@ -505,7 +504,9 @@ def reset( obs = {} info = {} - seed_ = {key: seed for key in self.envs} if seed is not None else {key: None for key in self.envs} + seed_: dict[str, int | None] = ( + {key: seed for key in self.envs} if seed is not None else {key: None for key in self.envs} + ) options_ = options if options is not None else {key: None for key in self.envs} reset_greenlets = {} diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index 1a45ff69..5383b7fa 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -2,7 +2,6 @@ import logging import typing from functools import partial -from typing import Type import gymnasium as gym import numpy as np @@ -86,7 +85,7 @@ def __call__( # type: ignore # ik = rcs_robotics_library._core.rl.RoboticsLibraryIK(robot_cfg.kinematic_model_path) robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) - env = SimEnv(simulation) + env: gym.Env = SimEnv(simulation) env = RobotWrapper(env, robot, control_mode) assert not ( hand_cfg is not None and gripper_cfg is not None @@ -129,9 +128,7 @@ def __call__( # type: ignore # ) if max_relative_movement is not None: env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) - env = CoverWrapper(env) - - return env + return CoverWrapper(env) class SimMultiEnvCreator(RCSHardwareEnvCreator): @@ -163,7 +160,8 @@ def __call__( # type: ignore cfg.add_postfix("_" + mid) robots[key] = rcs.sim.SimRobot(sim=simulation, ik=ik, cfg=cfg) - envs = {} + envs: dict[str, gym.Env] = {} + env: gym.Env for key, mid in name2id.items(): env = SimEnv(simulation) env = RobotWrapper(env, robots[key], control_mode) @@ -188,8 +186,7 @@ def __call__( # type: ignore BaseCameraSet, SimCameraSet(simulation, cameras, physical_units=True, render_on_demand=True) ) env = CameraSetWrapper(env, camera_set, include_depth=True) - env = CoverWrapper(env) - return env + return CoverWrapper(env) class SimTaskEnvCreator(EnvCreator): @@ -257,7 +254,7 @@ def __call__( # type: ignore ) env_rel = random_env(env_rel) if mode == "gripper": - env_rel = PickCubeSuccessWrapper(env_rel, cube_joint_name=obj_joint_name) + env_rel = PickCubeSuccessWrapper(env_rel, cube_geom_name=obj_joint_name) if render_mode == "human": env_rel.get_wrapper_attr("sim").open_gui() diff --git a/python/rcs/sim/egl_bootstrap.py b/python/rcs/sim/egl_bootstrap.py index 5a016ae9..57884bde 100644 --- a/python/rcs/sim/egl_bootstrap.py +++ b/python/rcs/sim/egl_bootstrap.py @@ -18,14 +18,13 @@ if name is not None: try: import mujoco.egl - import rcs._core as _cxx from mujoco.egl import GLContext _egl = ctypes.CDLL(name, mode=os.RTLD_LOCAL | os.RTLD_NOW) _addr_make_current = ctypes.cast(_egl.eglMakeCurrent, ctypes.c_void_p).value _ctx = GLContext(max_width=3840, max_height=2160) - _egl_display = mujoco.egl.EGL_DISPLAY.address - _egl_context = _ctx._context.address + _egl_display = int(mujoco.egl.EGL_DISPLAY.address) + _egl_context = int(_ctx._context.address) _egl_available = True except Exception: pass @@ -37,4 +36,6 @@ def bootstrap(): import rcs._core as _cxx assert _addr_make_current is not None + assert _egl_display is not None + assert _egl_context is not None _cxx.common._bootstrap_egl(_addr_make_current, _egl_display, _egl_context) diff --git a/python/tests/test_multi_sim.py b/python/tests/test_multi_sim.py index 55b301be..9f6746be 100644 --- a/python/tests/test_multi_sim.py +++ b/python/tests/test_multi_sim.py @@ -78,16 +78,16 @@ def test_sim_stepped_once_per_multi_step(self, multi_env): # First step from home — physics moves the robot some amount actions = {key: JointsDictType(joints=home[key]) for key in ROBOT2ID} obs1, _, _, _, _ = multi_env.step(actions) - delta1 = max(np.linalg.norm(obs1[k]["joints"] - home[k]) for k in ROBOT2ID) + delta1 = max(float(np.linalg.norm(obs1[k]["joints"] - home[k])) for k in ROBOT2ID) # Second step with same target — robot is now closer to or past target. # If the sim were stepped twice per call, delta2 would be >> delta1. obs2, _, _, _, info = multi_env.step(actions) - delta2 = max(np.linalg.norm(obs2[k]["joints"] - home[k]) for k in ROBOT2ID) + delta2 = max(float(np.linalg.norm(obs2[k]["joints"] - home[k])) for k in ROBOT2ID) for key in ROBOT2ID: assert info[key]["ik_success"], f"IK failed for {key!r}" - # delta2 should not be dramatically larger than delta1 (at most 3×) + # delta2 should not be dramatically larger than delta1 (at most 3x) assert ( delta2 < delta1 * 3 ), f"Second step drifted {delta2:.3f} vs first {delta1:.3f} — sim may be double-stepped"