From 86e00163b3cfb122dba8dcf9641ceb58cc383045 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Wed, 11 Sep 2024 17:07:13 +0200 Subject: [PATCH 1/2] fix(factories): camera in coll guard, relative env usage --- python/rcsss/envs/typed_factory.py | 69 ++++++++++++++---------------- 1 file changed, 33 insertions(+), 36 deletions(-) diff --git a/python/rcsss/envs/typed_factory.py b/python/rcsss/envs/typed_factory.py index 6287c820..dd74bc23 100644 --- a/python/rcsss/envs/typed_factory.py +++ b/python/rcsss/envs/typed_factory.py @@ -131,11 +131,7 @@ def produce_env_sim_tquart_gripper_camera_cg( mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id ) env_sim_tquart_gripper_cam_cg = CollisionGuard.env_from_xml_paths( - env_sim_tquart_gripper_cam, - mjcf_path, - urdf_path, - gripper=True, - check_home_collision=False, + env_sim_tquart_gripper_cam, mjcf_path, urdf_path, gripper=True, check_home_collision=False, camera=True ) return typing.cast( gym.Env[ObsArmsGrCam, TQuartDictType], @@ -215,11 +211,7 @@ def produce_env_sim_trpy_gripper_camera_cg( mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id ) env_sim_trpy_gripper_cam_cg = CollisionGuard.env_from_xml_paths( - env_sim_trpy_gripper_cam, - mjcf_path, - urdf_path, - gripper=True, - check_home_collision=False, + env_sim_trpy_gripper_cam, mjcf_path, urdf_path, gripper=True, check_home_collision=False, camera=True ) return typing.cast( gym.Env[ObsArmsGrCam, TRPYDictType], @@ -299,11 +291,7 @@ def produce_env_sim_joints_gripper_camera_cg( mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id ) env_sim_joints_gripper_cam_cg = CollisionGuard.env_from_xml_paths( - env_sim_joints_gripper_cam, - mjcf_path, - urdf_path, - gripper=True, - check_home_collision=False, + env_sim_joints_gripper_cam, mjcf_path, urdf_path, gripper=True, check_home_collision=False, camera=True ) return typing.cast( gym.Env[ObsArmsGrCam, JointsDictType], @@ -358,6 +346,27 @@ def produce_env_hw_joints_gripper( return typing.cast(gym.Env[ObsArmsGr, JointsDictType], env_hw_joints_gripper) +def produce_env_hw_joints_gripper_cg( + ip: str, urdf_path: str, mjcf_path: str, gripper_cfg: FHConfig, cfg_path: str +) -> gym.Env[ObsArmsGr, JointsDictType]: + env_hw_joints_gripper = produce_env_hw_joints_gripper(ip, urdf_path, gripper_cfg, cfg_path) + env_hw_joints_gripper_cg = CollisionGuard.env_from_xml_paths( + env_hw_joints_gripper, mjcf_path, urdf_path, gripper=True, check_home_collision=False, camera=True + ) + return typing.cast( + gym.Env[ObsArmsGr, JointsDictType], + env_hw_joints_gripper_cg, + ) + + +def produce_env_hw_joints_gripper_cg_rel( + ip: str, urdf_path: str, mjcf_path: str, gripper_cfg: FHConfig, cfg_path: str +) -> gym.Env[ObsArmsGr, LimitedJointsRelDictType]: + env_hw_joints_gripper_cg = produce_env_hw_joints_gripper_cg(ip, urdf_path, mjcf_path, gripper_cfg, cfg_path) + env_hw_joints_gripper_cg_rel = RelativeActionSpace(env_hw_joints_gripper_cg) + return typing.cast(gym.Env[ObsArmsGr, LimitedJointsRelDictType], env_hw_joints_gripper_cg_rel) + + def produce_env_hw_joints_gripper_camera( ip: str, urdf_path: str, gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, cfg_path: str ) -> (gym.Env)[ObsArmsGrCam, JointsDictType]: @@ -380,7 +389,7 @@ def produce_env_hw_joints_gripper_camera_cg( ) -> gym.Env[ObsArmsGrCam, JointsDictType]: env_hw_joints_gripper_cam = produce_env_hw_joints_gripper_camera(ip, urdf_path, gripper_cfg, cam_cfg, cfg_path) env_hw_joints_gripper_cam_cg = CollisionGuard.env_from_xml_paths( - env_hw_joints_gripper_cam, mjcf_path, urdf_path, gripper=True, check_home_collision=False + env_hw_joints_gripper_cam, mjcf_path, urdf_path, gripper=True, check_home_collision=False, camera=True ) return typing.cast(gym.Env[ObsArmsGrCam, JointsDictType], env_hw_joints_gripper_cam_cg) @@ -388,15 +397,11 @@ def produce_env_hw_joints_gripper_camera_cg( def produce_env_hw_joints_gripper_camera_cg_rel( ip: str, urdf_path: str, mjcf_path: str, gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, cfg_path: str ) -> gym.Env[ObsArmsGrCam, LimitedJointsRelDictType]: - env_hw_joints_gripper_cam = produce_env_hw_joints_gripper_camera(ip, urdf_path, gripper_cfg, cam_cfg, cfg_path) - env_hw_joints_gripper_cam_cg = CollisionGuard.env_from_xml_paths( - env_hw_joints_gripper_cam, - mjcf_path, - urdf_path, - gripper=True, - check_home_collision=False, + env_hw_joints_gripper_cam_cg = produce_env_hw_joints_gripper_camera_cg( + ip, urdf_path, mjcf_path, gripper_cfg, cam_cfg, cfg_path ) - return typing.cast(gym.Env[ObsArmsGrCam, LimitedJointsRelDictType], env_hw_joints_gripper_cam_cg) + env_hw_joints_gripper_cam_cg_rel = RelativeActionSpace(env_hw_joints_gripper_cam_cg) + return typing.cast(gym.Env[ObsArmsGrCam, LimitedJointsRelDictType], env_hw_joints_gripper_cam_cg_rel) def produce_env_hw_trpy(ip: str, urdf_path: str, cfg_path: str) -> gym.Env[ArmObsType, TRPYDictType]: @@ -435,7 +440,7 @@ def produce_env_hw_trpy_gripper_camera_cg( ) -> gym.Env[ObsArmsGrCam, TRPYDictType]: env_hw_trpy_gripper_cam = produce_env_hw_trpy_gripper_camera(ip, urdf_path, gripper_cfg, cam_cfg, cfg_path) env_hw_trpy_gripper_cam_cg = CollisionGuard.env_from_xml_paths( - env_hw_trpy_gripper_cam, mjcf_path, urdf_path, gripper=True, check_home_collision=False + env_hw_trpy_gripper_cam, mjcf_path, urdf_path, gripper=True, check_home_collision=False, camera=True ) return typing.cast(gym.Env[ObsArmsGrCam, TRPYDictType], env_hw_trpy_gripper_cam_cg) @@ -445,11 +450,7 @@ def produce_env_hw_trpy_gripper_camera_cg_rel( ) -> gym.Env[ObsArmsGrCam, LimitedTRPYRelDictType]: env_hw_trpy_gripper_cam = produce_env_hw_trpy_gripper_camera(ip, urdf_path, gripper_cfg, cam_cfg, cfg_path) env_hw_joints_gripper_cam_cg = CollisionGuard.env_from_xml_paths( - env_hw_trpy_gripper_cam, - mjcf_path, - urdf_path, - gripper=True, - check_home_collision=False, + env_hw_trpy_gripper_cam, mjcf_path, urdf_path, gripper=True, check_home_collision=False, camera=True ) return typing.cast(gym.Env[ObsArmsGrCam, LimitedTRPYRelDictType], env_hw_joints_gripper_cam_cg) @@ -490,7 +491,7 @@ def produce_env_hw_tquart_gripper_camera_cg( ) -> gym.Env[ObsArmsGrCam, TQuartDictType]: env_hw_tquart_gripper_cam = produce_env_hw_tquart_gripper_camera(ip, urdf_path, gripper_cfg, cam_cfg, cfg_path) env_hw_tquart_gripper_cam_cg = CollisionGuard.env_from_xml_paths( - env_hw_tquart_gripper_cam, mjcf_path, urdf_path, gripper=True, check_home_collision=False + env_hw_tquart_gripper_cam, mjcf_path, urdf_path, gripper=True, check_home_collision=False, camera=True ) return typing.cast(gym.Env[ObsArmsGrCam, TQuartDictType], env_hw_tquart_gripper_cam_cg) @@ -502,11 +503,7 @@ def produce_env_hw_tquart_gripper_camera_cg_rel( ip, urdf_path, mjcf_path, gripper_cfg, cam_cfg, cfg_path ) env_hw_tquart_gripper_cam_cg = CollisionGuard.env_from_xml_paths( - env_hw_tquart_gripper_cam, - mjcf_path, - urdf_path, - gripper=True, - check_home_collision=False, + env_hw_tquart_gripper_cam, mjcf_path, urdf_path, gripper=True, check_home_collision=False, camera=True ) return typing.cast(gym.Env[ObsArmsGrCam, LimitedTQuartRelDictType], env_hw_tquart_gripper_cam_cg) From 52782da0b4fe61b4367c965b05437850024ebf32 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Wed, 11 Sep 2024 17:09:21 +0200 Subject: [PATCH 2/2] refactor(env): moved examples into own folder #111 and simplified them --- python/examples/__init__.py | 0 python/examples/cartesian_control.py | 53 +++++++++++++ python/examples/common.py | 107 +++++++++++++++++++++++++++ python/examples/joint_control.py | 47 ++++++++++++ python/rcsss/desk.py | 8 ++ python/rcsss/envs/base.py | 11 ++- python/rcsss/envs/hw.py | 91 +---------------------- python/rcsss/envs/sim.py | 58 +-------------- 8 files changed, 225 insertions(+), 150 deletions(-) create mode 100644 python/examples/__init__.py create mode 100644 python/examples/cartesian_control.py create mode 100644 python/examples/common.py create mode 100644 python/examples/joint_control.py diff --git a/python/examples/__init__.py b/python/examples/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/python/examples/cartesian_control.py b/python/examples/cartesian_control.py new file mode 100644 index 00000000..24495a51 --- /dev/null +++ b/python/examples/cartesian_control.py @@ -0,0 +1,53 @@ +import logging + +from dotenv import dotenv_values +from rcsss.desk import FCI, Desk, DummyResourceManager +from rcsss.envs.base import ControlMode, RobotInstance + +from common import hw_env_rel, sim_env_rel + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + +ROBOT_IP = "192.168.101.1" +ROBOT_INSTANCE = RobotInstance.SIMULATION + + +""" +Create a .env file in the same directory as this file with the following content: +FR3_USERNAME= +FR3_PASSWORD= +""" + + +def main(): + if ROBOT_INSTANCE == RobotInstance.HARDWARE: + env_rel = hw_env_rel(ROBOT_IP, ControlMode.CARTESIAN_TQuart) + creds = dotenv_values() + resource_manger = FCI( + Desk(ROBOT_IP, creds["FR3_USERNAME"], creds["FR3_USERNAME"]), unlock=False, lock_when_done=False + ) + else: + env_rel = sim_env_rel(ControlMode.CARTESIAN_TQuart) + resource_manger = DummyResourceManager() + + print(env_rel.unwrapped.robot.get_cartesian_position()) + + with resource_manger: + for _ in range(10): + for _ in range(10): + act = {"tquart": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 0} + obs, reward, terminated, truncated, info = env_rel.step(act) + if truncated or terminated: + logger.info("Truncated or terminated!") + return + for _ in range(10): + act = {"tquart": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": 1} + obs, reward, terminated, truncated, info = env_rel.step(act) + if truncated or terminated: + logger.info("Truncated or terminated!") + return + + +if __name__ == "__main__": + main() diff --git a/python/examples/common.py b/python/examples/common.py new file mode 100644 index 00000000..b9fe99e9 --- /dev/null +++ b/python/examples/common.py @@ -0,0 +1,107 @@ +import logging +import sys + +import gymnasium as gym +import numpy as np +import rcsss +from rcsss import sim +from rcsss._core.hw import FR3Config +from rcsss._core.sim import CameraType +from rcsss.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig +from rcsss.envs.base import ( + CameraSetWrapper, + ControlMode, + FR3Env, + GripperWrapper, + LimitedJointsRelDictType, + ObsArmsGr, + ObsArmsGrCam, + RelativeActionSpace, +) +from rcsss.envs.hw import FR3HW +from rcsss.envs.sim import CollisionGuard, FR3Sim + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + + +def hw_env_rel(ip: str, control_mode: ControlMode) -> gym.Env[ObsArmsGr, LimitedJointsRelDictType]: + if "lab" not in rcsss.scenes: + logger.error("This pip package was not built with the UTN lab models, aborting.") + sys.exit() + robot = rcsss.hw.FR3(ip, str(rcsss.scenes["lab"].parent / "fr3.urdf")) + robot_cfg = FR3Config() + robot.set_parameters(robot_cfg) + # the robot itself is always controlled in joint space with the robotics library IK + env = FR3Env(robot, ControlMode.JOINTS) + env_hw: gym.Env = FR3HW(env) + gripper_cfg = rcsss.hw.FHConfig() + gripper_cfg.epsilon_inner = gripper_cfg.epsilon_outer = 0.1 + gripper_cfg.speed = 0.1 + gripper_cfg.force = 10 + gripper = rcsss.hw.FrankaHand(ip, gripper_cfg) + env_hw = GripperWrapper(env_hw, gripper, binary=True) + + env_cg: gym.Env = CollisionGuard.env_from_xml_paths( + env_hw, + str(rcsss.scenes["fr3_empty_world"]), + str(rcsss.scenes["lab"].parent / "fr3.urdf"), + gripper=True, + check_home_collision=False, + camera=True, + control_mode=control_mode, + tcp_offset=rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()), + ) + env_rel = RelativeActionSpace(env_cg, max_mov=np.deg2rad(5)) + + # if you have a realsense camera: + # cameras = { + # "side": BaseCameraConfig(identifier="243522070385"), + # } + + # cam_cfg = RealSenseSetConfig( + # cameras=cameras, + # resolution_width=1280, + # resolution_height=720, + # frame_rate=15, + # enable_imu=False, + # enable_ir=True, + # enable_ir_emitter=False, + # ) + + # camera_set = RealSenseCameraSet(cam_cfg) + # camera_set.start() + # env_rel = CameraSetWrapper(env_rel, camera_set) + + env_rel.reset() + return env_rel + + +def sim_env_rel(control_mode: ControlMode) -> gym.Env[ObsArmsGrCam, LimitedJointsRelDictType]: + if "lab" not in rcsss.scenes: + logger.error("This pip package was not built with the UTN lab models, aborting.") + sys.exit() + simulation = sim.Sim(rcsss.scenes["fr3_empty_world"]) + robot = rcsss.sim.FR3(simulation, "0", str(rcsss.scenes["lab"].parent / "fr3.urdf")) + cfg = sim.FR3Config() + cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) + cfg.ik_duration_in_milliseconds = 300 + cfg.realtime = False + robot.set_parameters(cfg) + # env = FR3Env(robot, ControlMode.CARTESIAN_TQuart) + env = FR3Env(robot, control_mode) + env_sim = FR3Sim(env, simulation) + cameras = { + "wrist": SimCameraConfig(identifier="eye-in-hand_0", type=int(CameraType.fixed), on_screen_render=False), + "default_free": SimCameraConfig(identifier="", type=int(CameraType.default_free), on_screen_render=True), + } + cam_cfg = SimCameraSetConfig(cameras=cameras, resolution_width=1280, resolution_height=720, frame_rate=10) + camera_set = SimCameraSet(simulation, cam_cfg) + env_cam: gym.Env = CameraSetWrapper(env_sim, camera_set) + + gripper_cfg = sim.FHConfig() + gripper = sim.FrankaHand(simulation, "0", gripper_cfg) + env_cam = GripperWrapper(env_cam, gripper, binary=True) + env_cam = RelativeActionSpace(env_cam, max_mov=np.deg2rad(5)) + env_cam.reset() + return env_cam diff --git a/python/examples/joint_control.py b/python/examples/joint_control.py new file mode 100644 index 00000000..fba3f7b8 --- /dev/null +++ b/python/examples/joint_control.py @@ -0,0 +1,47 @@ +import logging + +from dotenv import dotenv_values +from rcsss.desk import FCI, Desk, DummyResourceManager +from rcsss.envs.base import ControlMode, RobotInstance + +from common import hw_env_rel, sim_env_rel + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + +ROBOT_IP = "192.168.101.1" +ROBOT_INSTANCE = RobotInstance.SIMULATION + + +""" +Create a .env file in the same directory as this file with the following content: +FR3_USERNAME= +FR3_PASSWORD= +""" + + +def main(): + if ROBOT_INSTANCE == RobotInstance.HARDWARE: + env_rel = hw_env_rel(ROBOT_IP, ControlMode.JOINTS) + creds = dotenv_values() + resource_manger = FCI( + Desk(ROBOT_IP, creds["FR3_USERNAME"], creds["FR3_USERNAME"]), unlock=False, lock_when_done=False + ) + else: + env_rel = sim_env_rel(ControlMode.JOINTS) + resource_manger = DummyResourceManager() + + with resource_manger: + for _ in range(10): + obs, info = env_rel.reset() + for _ in range(10): + act = env_rel.action_space.sample() + obs, reward, terminated, truncated, info = env_rel.step(act) + if truncated or terminated: + logger.info("Truncated or terminated!") + return + logger.info(act["gripper"], obs["gripper"]) + + +if __name__ == "__main__": + main() diff --git a/python/rcsss/desk.py b/python/rcsss/desk.py index fe3a0c37..ed464341 100644 --- a/python/rcsss/desk.py +++ b/python/rcsss/desk.py @@ -78,6 +78,14 @@ class Token: token: str = "" +class DummyResourceManager: + def __enter__(self): + pass + + def __exit__(self, *args): + pass + + class Desk: """ Connects to the control unit running the web-based Desk interface diff --git a/python/rcsss/envs/base.py b/python/rcsss/envs/base.py index 955746b8..c0e453b6 100644 --- a/python/rcsss/envs/base.py +++ b/python/rcsss/envs/base.py @@ -132,9 +132,14 @@ class ObsArmsGrCamCG(ArmObsType, GripperDictType, CameraDictType): class ControlMode(Enum): - JOINTS = 1 - CARTESIAN_TRPY = 2 - CARTESIAN_TQuart = 3 + JOINTS = auto() + CARTESIAN_TRPY = auto() + CARTESIAN_TQuart = auto() + + +class RobotInstance(Enum): + HARDWARE = auto() + SIMULATION = auto() class FR3Env(gym.Env): diff --git a/python/rcsss/envs/hw.py b/python/rcsss/envs/hw.py index db396c5e..5f7ea35b 100644 --- a/python/rcsss/envs/hw.py +++ b/python/rcsss/envs/hw.py @@ -2,12 +2,8 @@ from typing import Any, SupportsFloat, cast import gymnasium as gym -import rcsss from rcsss import hw -from rcsss.config import read_config_yaml -from rcsss.desk import FCI, Desk -from rcsss.envs.base import ControlMode, FR3Env, GripperWrapper, RelativeActionSpace -from rcsss.envs.sim import CollisionGuard +from rcsss.envs.base import FR3Env _logger = logging.getLogger(__name__) @@ -34,88 +30,3 @@ def reset( ) -> tuple[dict[str, Any], dict[str, Any]]: self.hw_robot.move_home() return super().reset(seed=seed, options=options) - - -def cartesian_example(ip: str, cfg_path: str): - cfg = read_config_yaml(cfg_path) - d = Desk(ip, cfg.hw.username, cfg.hw.password) - with FCI(d, unlock=True, lock_when_done=True): - logger = logging.getLogger(__name__) - robot = hw.FR3(ip, str(rcsss.scenes["lab"].parent / "fr3.urdf")) - env = FR3Env(robot, ControlMode.JOINTS) - env_hw: gym.Env = FR3HW(env) - gripper_cfg = hw.FHConfig() - gripper_cfg.epsilon_inner = gripper_cfg.epsilon_outer = 0.1 - gripper_cfg.speed = 0.1 - gripper_cfg.force = 10 - gripper = hw.FrankaHand(ip, gripper_cfg) - env_hw = GripperWrapper(env_hw, gripper) - - env_cam: gym.Env = CollisionGuard.env_from_xml_paths( - env_hw, - str(rcsss.scenes["fr3_empty_world"]), - str(rcsss.scenes["lab"].parent / "fr3.urdf"), - gripper=True, - check_home_collision=False, - camera=True, - control_mode=ControlMode.CARTESIAN_TQuart, - tcp_offset=rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()), - ) - env_cam = RelativeActionSpace(env_cam) - obs, info = env_cam.reset() - print(env_cam.unwrapped.robot.get_cartesian_position()) - for _ in range(10): - for _ in range(10): - act = {"tquart": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 0} - obs, reward, terminated, truncated, info = env_cam.step(act) - if truncated or terminated: - logger.info("Truncated or terminated!") - return - for _ in range(10): - act = {"tquart": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": 0} - obs, reward, terminated, truncated, info = env_cam.step(act) - if truncated or terminated: - logger.info("Truncated or terminated!") - return - - -def joints_example(ip: str, cfg_path: str): - cfg = read_config_yaml(cfg_path) - d = Desk(ip, cfg.hw.username, cfg.hw.password) - with FCI(d, unlock=True, lock_when_done=True): - logger = logging.getLogger(__name__) - robot = hw.FR3(ip, str(rcsss.scenes["lab"].parent / "fr3.urdf")) - env = FR3Env(robot, ControlMode.JOINTS) - env_hw: gym.Env = FR3HW(env) - gripper_cfg = hw.FHConfig() - gripper_cfg.epsilon_inner = gripper_cfg.epsilon_outer = 0.1 - gripper_cfg.speed = 0.1 - gripper_cfg.force = 10 - gripper = hw.FrankaHand(ip, gripper_cfg) - env_hw = GripperWrapper(env_hw, gripper) - - env_cam: gym.Env = CollisionGuard.env_from_xml_paths( - env_hw, - str(rcsss.scenes["fr3_empty_world"]), - str(rcsss.scenes["lab"].parent / "fr3.urdf"), - gripper=True, - check_home_collision=False, - camera=True, - control_mode=ControlMode.JOINTS, - tcp_offset=rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()), - ) - env_cam = RelativeActionSpace(env_cam) - for _ in range(10): - obs, info = env_cam.reset() - for _ in range(10): - act = env_cam.action_space.sample() - obs, reward, terminated, truncated, info = env_cam.step(act) - if truncated or terminated: - logger.info("Truncated or terminated!") - return - logger.info(act["gripper"], obs["gripper"]) - - -if __name__ == "__main__": - cartesian_example("192.168.101.1", "config.yaml") - # joints_example("192.168.101.1", "config.yaml") diff --git a/python/rcsss/envs/sim.py b/python/rcsss/envs/sim.py index 24b51d23..ff41338c 100644 --- a/python/rcsss/envs/sim.py +++ b/python/rcsss/envs/sim.py @@ -1,5 +1,4 @@ import logging -import sys from typing import Any, SupportsFloat, cast import gymnasium as gym @@ -7,13 +6,7 @@ from rcsss import sim from rcsss._core.sim import CameraType from rcsss.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig -from rcsss.envs.base import ( - CameraSetWrapper, - ControlMode, - FR3Env, - GripperWrapper, - RelativeActionSpace, -) +from rcsss.envs.base import CameraSetWrapper, ControlMode, FR3Env, GripperWrapper class FR3Sim(gym.Wrapper): @@ -160,52 +153,3 @@ def env_from_xml_paths( camera_set = SimCameraSet(simulation, cam_cfg) c_env = CameraSetWrapper(c_env, camera_set) return cls(env, simulation, FR3Sim(c_env, simulation), check_home_collision, to_joint_control) - - -if __name__ == "__main__": - logger = logging.getLogger(__name__) - if "lab" not in rcsss.scenes: - logger.error("This pip package was not built with the UTN lab models, aborting.") - sys.exit() - simulation = sim.Sim(rcsss.scenes["fr3_empty_world"]) - robot = rcsss.sim.FR3(simulation, "0", str(rcsss.scenes["lab"].parent / "fr3.urdf")) - cfg = sim.FR3Config() - cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) - cfg.ik_duration_in_milliseconds = 300 - cfg.realtime = False - robot.set_parameters(cfg) - # env = FR3Env(robot, ControlMode.CARTESIAN_TQuart) - env = FR3Env(robot, ControlMode.JOINTS) - env_sim = FR3Sim(env, simulation) - cameras = { - "wrist": SimCameraConfig(identifier="eye-in-hand_0", type=int(CameraType.fixed), on_screen_render=False), - "default_free": SimCameraConfig(identifier="", type=int(CameraType.default_free), on_screen_render=True), - } - cam_cfg = SimCameraSetConfig(cameras=cameras, resolution_width=1280, resolution_height=720, frame_rate=1) - camera_set = SimCameraSet(simulation, cam_cfg) - env_cam: gym.Env = CameraSetWrapper(env_sim, camera_set) - - gripper_cfg = sim.FHConfig() - gripper = sim.FrankaHand(simulation, "0", gripper_cfg) - env_cam = GripperWrapper(env_cam, gripper) - env_cam = CollisionGuard.env_from_xml_paths( - env_cam, - str(rcsss.scenes["fr3_empty_world"]), - str(rcsss.scenes["lab"].parent / "fr3.urdf"), - gripper=True, - check_home_collision=False, - camera=True, - tcp_offset=cfg.tcp_offset, - control_mode=ControlMode.CARTESIAN_TQuart, - ) - env_cam = RelativeActionSpace(env_cam) - obs, info = env_cam.reset() - for _ in range(100): - act = env_cam.action_space.sample() - # act = {"tquart": np.array([0, 0, 0, 0, 0, 0, 1]), "gripper": i%2} - # act["gripper"] = i % 2 - obs, reward, terminated, truncated, info = env_cam.step(act) - if truncated or terminated: - logger.info("Truncated or terminated!") - env_cam.reset() - logger.info(act["gripper"], obs["gripper"])