diff --git a/README.md b/README.md index 666f7870..ad374772 100644 --- a/README.md +++ b/README.md @@ -48,10 +48,12 @@ from rcs.camera.sim import SimCameraSet from rcs.envs.base import ( CameraSetWrapper, ControlMode, + CoverWrapper, GripperWrapper, RelativeActionSpace, RelativeTo, - RobotEnv, + RobotWrapper, + SimEnv, ) from rcs.envs.sim import GripperWrapperSim, RobotSimWrapper from rcs.envs.utils import ( @@ -82,21 +84,23 @@ if __name__ == "__main__": # base env robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) - env: gym.Env = RobotEnv(robot, ControlMode.CARTESIAN_TQuat) + env: gym.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) + env = CoverWrapper(env) env.get_wrapper_attr("sim").open_gui() # wait for gui to open @@ -104,14 +108,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/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/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/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..6e2dd5f3 100644 --- a/examples/fr3/fr3_readme.py +++ b/examples/fr3/fr3_readme.py @@ -7,10 +7,12 @@ from rcs.envs.base import ( CameraSetWrapper, ControlMode, + CoverWrapper, GripperWrapper, RelativeActionSpace, RelativeTo, - RobotEnv, + RobotWrapper, + SimEnv, ) from rcs.envs.sim import GripperWrapperSim, RobotSimWrapper from rcs.envs.utils import ( @@ -41,14 +43,15 @@ # base env robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) - env: gym.Env = RobotEnv(robot, ControlMode.CARTESIAN_TQuat) + env: gym.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) @@ -56,6 +59,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 @@ -63,7 +67,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..569cf483 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/creators.py +++ b/extensions/rcs_fr3/src/rcs_fr3/creators.py @@ -11,12 +11,14 @@ from rcs.envs.base import ( CameraSetWrapper, ControlMode, + CoverWrapper, GripperWrapper, HandWrapper, + HardwareEnv, MultiRobotWrapper, RelativeActionSpace, RelativeTo, - RobotEnv, + RobotWrapper, ) from rcs.envs.creators import RCSHardwareEnvCreator from rcs.hand.tilburg_hand import TilburgHand @@ -91,7 +93,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: gym.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): @@ -123,8 +126,7 @@ def __call__( # type: ignore # ) if relative_to != RelativeTo.NONE: env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) - - return env + return CoverWrapper(env) class RCSFR3MultiEnvCreator(RCSHardwareEnvCreator): @@ -152,9 +154,11 @@ 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: 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) @@ -170,7 +174,7 @@ def __call__( # type: ignore camera_set.wait_for_frames() logger.info("CameraSet started") env = CameraSetWrapper(env, camera_set) - return env + return CoverWrapper(env) class RCSFR3DefaultEnvCreator(RCSHardwareEnvCreator): 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..b5d452ee 100644 --- a/extensions/rcs_panda/src/rcs_panda/creators.py +++ b/extensions/rcs_panda/src/rcs_panda/creators.py @@ -7,12 +7,14 @@ from rcs.envs.base import ( CameraSetWrapper, ControlMode, + CoverWrapper, GripperWrapper, HandWrapper, + HardwareEnv, MultiRobotWrapper, RelativeActionSpace, RelativeTo, - RobotEnv, + RobotWrapper, ) from rcs.envs.creators import RCSHardwareEnvCreator from rcs.hand.tilburg_hand import TilburgHand @@ -68,7 +70,9 @@ def __call__( # type: ignore robot = hw.Franka(ip, ik) robot.set_config(robot_cfg) - env: gym.Env = RobotEnv( + env: gym.Env = HardwareEnv() + env = RobotWrapper( + env, robot, ControlMode.JOINTS if collision_guard is not None else control_mode, home_on_reset=True, @@ -103,12 +107,12 @@ def __call__( # type: ignore # ) if max_relative_movement is not None: env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) - - return env + return CoverWrapper(env) class RCSPandaMultiEnvCreator(RCSHardwareEnvCreator): def __call__( # type: ignore + self, ips: list[str], control_mode: ControlMode, robot_cfg: hw.PandaConfig, @@ -130,9 +134,11 @@ 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: 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) @@ -148,4 +154,4 @@ def __call__( # type: ignore camera_set.wait_for_frames() logger.info("CameraSet started") env = CameraSetWrapper(env, camera_set) - return env + return CoverWrapper(env) 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..6c541bec 100644 --- a/extensions/rcs_so101/src/rcs_so101/creators.py +++ b/extensions/rcs_so101/src/rcs_so101/creators.py @@ -5,10 +5,12 @@ from rcs.envs.base import ( CameraSetWrapper, ControlMode, + CoverWrapper, GripperWrapper, + HardwareEnv, RelativeActionSpace, RelativeTo, - RobotEnv, + RobotWrapper, ) from rcs.envs.creators import RCSHardwareEnvCreator from rcs_so101 import SO101IK @@ -33,7 +35,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: gym.Env = HardwareEnv() + env = RobotWrapper(env, robot, control_mode, home_on_reset=True) gripper = SO101Gripper(robot._hf_robot, robot) env = GripperWrapper(env, gripper, binary=False) @@ -46,8 +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) - - 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_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/extensions/rcs_ur5e/src/rcs_ur5e/creators.py b/extensions/rcs_ur5e/src/rcs_ur5e/creators.py index f56353d8..3cc6e2c0 100644 --- a/extensions/rcs_ur5e/src/rcs_ur5e/creators.py +++ b/extensions/rcs_ur5e/src/rcs_ur5e/creators.py @@ -5,10 +5,12 @@ from rcs.envs.base import ( CameraSetWrapper, ControlMode, + CoverWrapper, GripperWrapper, + HardwareEnv, RelativeActionSpace, RelativeTo, - RobotEnv, + RobotWrapper, ) from rcs.envs.creators import RCSHardwareEnvCreator from rcs_ur5e.hw import RobotiQGripper, UR5e, UR5eConfig @@ -36,7 +38,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: gym.Env = HardwareEnv() + env = RobotWrapper(env, robot, control_mode, home_on_reset=True) gripper = RobotiQGripper(ip) env = GripperWrapper(env, gripper, binary=True) @@ -49,5 +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) - - 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 1a63d355..9a97b3b9 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py +++ b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py @@ -7,10 +7,12 @@ from rcs.envs.base import ( CameraSetWrapper, ControlMode, + CoverWrapper, HandWrapper, + HardwareEnv, RelativeActionSpace, RelativeTo, - RobotEnv, + RobotWrapper, ) from rcs.envs.creators import RCSHardwareEnvCreator from rcs.hand.tilburg_hand import THConfig, TilburgHand @@ -34,7 +36,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: gym.Env = HardwareEnv() + env = RobotWrapper(env, robot, control_mode, home_on_reset=True) if camera_set is not None: camera_set.start() @@ -47,5 +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) - - return env + return CoverWrapper(env) 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/__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"), 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/base.py b/python/rcs/envs/base.py index 9ae52a89..fb6cf410 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -7,7 +7,8 @@ import gymnasium as gym import numpy as np -from rcs._core.common import Hand +from greenlet import getcurrent, greenlet +from rcs._core.common import Hand, RobotPlatform from rcs.camera.interface import BaseCameraSet from rcs.envs.space_utils import ( ActObsInfoWrapper, @@ -20,8 +21,10 @@ get_space, get_space_keys, ) +from rcs.utils import SimpleFrameRate from rcs import common +from rcs import sim as simulation _logger = logging.getLogger(__name__) @@ -197,8 +200,81 @@ def get_home_position(robot: common.Robot) -> np.ndarray: return common.robots_meta_config(robot.get_config().robot_type).q_home -class RobotEnv(gym.Env): - """Joint Gym Environment for a single robot arm. +class BaseEnv(gym.Env): + PLATFORM: RobotPlatform + + 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 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]]: + if self.main_greenlet is not None: + self.main_greenlet.switch() + else: + 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) + + +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 +284,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 +324,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 +333,110 @@ 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) + 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_dict[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_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) + 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_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) + 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_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()) + 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.prev_action = None 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 + self.sim: simulation.Sim | 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() + 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" + 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 +452,27 @@ 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 +480,17 @@ 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. + ob, r, t, tr, info[key] = step_greenlets[key].switch() + 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 +499,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_: 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 = {} + 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: apply state from rested wrappers + assert isinstance(self.lead_env, SimEnv) + self.lead_env.apply_sim_state() + 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 +569,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 +605,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 +613,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 +639,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 +651,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 +669,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 +684,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 +728,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 +785,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 +825,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 +885,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 +953,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 +964,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 +979,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 +995,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 4a693bad..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 @@ -13,12 +12,14 @@ from rcs.envs.base import ( CameraSetWrapper, ControlMode, + CoverWrapper, GripperWrapper, HandWrapper, 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: gym.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( @@ -130,8 +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) - - return env + return CoverWrapper(env) class SimMultiEnvCreator(RCSHardwareEnvCreator): @@ -146,7 +143,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: @@ -161,30 +157,36 @@ 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 = {} + envs: dict[str, gym.Env] = {} + env: gym.Env 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_id(mid) + 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) ) env = CameraSetWrapper(env, camera_set, include_depth=True) - return env + return CoverWrapper(env) class SimTaskEnvCreator(EnvCreator): @@ -249,10 +251,10 @@ 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) + env_rel = PickCubeSuccessWrapper(env_rel, cube_geom_name=obj_joint_name) if render_mode == "human": env_rel.get_wrapper_attr("sim").open_gui() @@ -331,7 +333,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..8b3ac8cf 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -1,18 +1,11 @@ import logging -from typing import Any, SupportsFloat, Type, cast +from typing import Any, cast import gymnasium as gym import numpy as np -from rcs.envs.base import ( - ControlMode, - GripperWrapper, - HandWrapper, - MultiRobotWrapper, - RobotEnv, -) +from rcs._core.common import RobotPlatform +from rcs.envs.base import GripperWrapper 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 +14,19 @@ 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 +34,23 @@ 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 + return observation, 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()}) - - 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 - - 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 - - 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() - 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 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 +64,21 @@ 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 +97,7 @@ 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_id(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) - 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 +114,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 +123,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 +145,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 +157,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 +167,27 @@ 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. - def __init__(self, env: gym.Env, simulation: sim.Sim, include_rotation: bool = False, cube_joint_name="box_joint"): - super().__init__(env, simulation) + Works only for single robot + """ + + 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 +195,26 @@ 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 +229,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,10 +250,8 @@ 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) - tcp_to_obj_dist = np.linalg.norm( - cube_pose.translation() - self.unwrapped.robot.get_cartesian_position().translation() - ) + 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()) 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. @@ -458,7 +274,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/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/python/rcs/ompl/mj_ompl.py b/python/rcs/ompl/mj_ompl.py index 7112b255..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 -from rcs.envs.base import RobotEnv 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..78030985 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.env.get_wrapper_attr("get_robot_obs")() @rpyc.exposed def unwrapped(self): diff --git a/python/rcs/sim/egl_bootstrap.py b/python/rcs/sim/egl_bootstrap.py index 6a3d9d5e..57884bde 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,33 @@ 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) +if name is not None: + try: + import mujoco.egl + 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 = 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 = int(mujoco.egl.EGL_DISPLAY.address) + _egl_context = int(_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 + 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 new file mode 100644 index 00000000..9f6746be --- /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(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(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 3x) + 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..2da01ab1 100644 --- a/python/tests/test_sim_envs.py +++ b/python/tests/test_sim_envs.py @@ -150,33 +150,6 @@ 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): """This class is for testing Tquat sim env functionalities""" @@ -270,33 +243,6 @@ 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): """This class is for testing Joints sim env functionalities""" @@ -359,32 +305,6 @@ 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): """ Check that an obvious collision is detected by the CollisionGuard diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 78726e2e..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_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 +529,8 @@ 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 +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_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);