From 9ae92bb7b4c4c61c0eadfdf45a83794dabe739ae Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 11 Jul 2024 12:13:29 +0200 Subject: [PATCH 01/22] feat: added first four typed env funcs --- python/rcsss/envs/typed_factory.py | 75 ++++++++++++++++++++++++++++++ 1 file changed, 75 insertions(+) create mode 100644 python/rcsss/envs/typed_factory.py diff --git a/python/rcsss/envs/typed_factory.py b/python/rcsss/envs/typed_factory.py new file mode 100644 index 00000000..4254aef8 --- /dev/null +++ b/python/rcsss/envs/typed_factory.py @@ -0,0 +1,75 @@ +import typing +from rcsss import sim +from rcsss._core.sim import CameraType, SimCameraSet, SimCameraSetConfig +from rcsss.camera.sim import SimCameraConfig +from rcsss.envs.base import ArmObsType, CameraSetWrapper, CartOrJointContType, ControlMode, FR3Env, JointsDictType, TQuartDictType, TRPYDictType +from rcsss.envs.sim import FR3Sim +import gymnasium as gym + + +def produce_env_sim( + mjcf_path: str, urdf_path: str, control_mode: ControlMode, cfg: sim.FR3Config, robot_id="0" +) -> gym.Env[ArmObsType, CartOrJointContType]: + simulation = sim.Sim(mjcf_path) + robot = sim.FR3(simulation, robot_id, urdf_path) + robot.set_parameters(cfg) + env = FR3Env(robot, control_mode) + env_sim = FR3Sim(env, simulation) + return typing.cast(gym.Env[ArmObsType, CartOrJointContType], FR3Sim(env_sim, simulation)) + + +def produce_env_sim_joints( + mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id="0" +) -> gym.Env[ArmObsType, JointsDictType]: + return typing.cast( + gym.Env[ArmObsType, JointsDictType], + produce_env_sim(mjcf_path, urdf_path, control_mode=ControlMode.JOINTS, cfg=cfg, robot_id=robot_id), + ) + +def produce_env_sim_trpy( + mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id="0" +) -> gym.Env[ArmObsType, TRPYDictType]: + return typing.cast( + gym.Env[ArmObsType, TRPYDictType], + produce_env_sim(mjcf_path, urdf_path, control_mode=ControlMode.CARTESIAN_TRPY, cfg=cfg, robot_id=robot_id), + ) + +def produce_env_sim_tquart( + mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id="0" +) -> gym.Env[ArmObsType, TQuartDictType]: + return typing.cast( + gym.Env[ArmObsType, TQuartDictType], + produce_env_sim(mjcf_path, urdf_path, control_mode=ControlMode.CARTESIAN_TQuart, cfg=cfg, robot_id=robot_id), + ) + +def produce_env_sim_tquart_gripper_camera_cg_rel( + mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id="0" +) -> gym.Env[ArmObsType, TQuartDictType]: + pass + +def produce_env_sim_tquart_gripper_camera_cg( + mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id="0" +) -> gym.Env[ArmObsType, TQuartDictType]: + pass + +def produce_env_sim_tquart_gripper_camera_rel( + mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id="0" +) -> gym.Env[ArmObsType, TQuartDictType]: + pass + +# ... + + +def produce_env_hw(): + pass + +def produce_env_hw_joints(): + pass + +# ... + +def produce_env_sim_joints_gripper_camera_cg_rel(): + pass + +# permutations base(sim|hw) x Control(joints|trpy|tquart) x Util(Gripper,Camera,CollisionGuard,RelativeAction|Gripper,Camera,CollisionGuard|Gripper,Camera,RelativeAction) +# 2 + 2x3 + 2x3x3 = 26 permutations \ No newline at end of file From 29b03892453bab04c34233d76670acb0deb0b018 Mon Sep 17 00:00:00 2001 From: suman1209 Date: Thu, 11 Jul 2024 15:43:20 +0200 Subject: [PATCH 02/22] feat(type_factories): added sim creation functions for sim, tquart --- python/rcsss/envs/base.py | 8 +++ python/rcsss/envs/typed_factory.py | 82 ++++++++++++++++++++++++++---- 2 files changed, 81 insertions(+), 9 deletions(-) diff --git a/python/rcsss/envs/base.py b/python/rcsss/envs/base.py index 1efb5548..ff354217 100644 --- a/python/rcsss/envs/base.py +++ b/python/rcsss/envs/base.py @@ -119,6 +119,14 @@ class ArmObsType(TQuartDictType, JointsDictType): ... LimitedCartOrJointContType: TypeAlias = LimitedTQuartRelDictType | LimitedJointsRelDictType | LimitedTRPYRelDictType +class ActionsRelative(LimitedJointsRelDictType): + pass + + +class ObsArmsGrCam(ArmObsType, GripperDictType, CameraDictType): + pass + + class ControlMode(Enum): JOINTS = 1 CARTESIAN_TRPY = 2 diff --git a/python/rcsss/envs/typed_factory.py b/python/rcsss/envs/typed_factory.py index 4254aef8..e39c86e0 100644 --- a/python/rcsss/envs/typed_factory.py +++ b/python/rcsss/envs/typed_factory.py @@ -1,4 +1,15 @@ import typing +import rcsss +from rcsss.envs.sim import CollisionGuard +from rcsss.envs.base import ( + CameraSetWrapper, + ControlMode, + FR3Env, + GripperWrapper, + RelativeActionSpace, + ObsArmsGrCam, + LimitedCartOrJointContType +) from rcsss import sim from rcsss._core.sim import CameraType, SimCameraSet, SimCameraSetConfig from rcsss.camera.sim import SimCameraConfig @@ -26,6 +37,7 @@ def produce_env_sim_joints( produce_env_sim(mjcf_path, urdf_path, control_mode=ControlMode.JOINTS, cfg=cfg, robot_id=robot_id), ) + def produce_env_sim_trpy( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id="0" ) -> gym.Env[ArmObsType, TRPYDictType]: @@ -34,6 +46,7 @@ def produce_env_sim_trpy( produce_env_sim(mjcf_path, urdf_path, control_mode=ControlMode.CARTESIAN_TRPY, cfg=cfg, robot_id=robot_id), ) + def produce_env_sim_tquart( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id="0" ) -> gym.Env[ArmObsType, TQuartDictType]: @@ -42,34 +55,85 @@ def produce_env_sim_tquart( produce_env_sim(mjcf_path, urdf_path, control_mode=ControlMode.CARTESIAN_TQuart, cfg=cfg, robot_id=robot_id), ) -def produce_env_sim_tquart_gripper_camera_cg_rel( + +def produce_env_sim_tquart_gripper_camera( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id="0" ) -> gym.Env[ArmObsType, TQuartDictType]: - pass + """Environment with tquart, grippter, camera, collision guard and relative limits""" + simulation = sim.Sim(mjcf_path) + env_sim = produce_env_sim_tquart(mjcf_path, urdf_path, cfg, robot_id) + gripper_cfg = sim.FHConfig() + gripper = sim.FrankaHand(simulation, "0", gripper_cfg) + env_sim = GripperWrapper(env_sim, gripper) + 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=640, resolution_height=480, frame_rate=50) + 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) + return typing.cast( + gym.Env[ObsArmsGrCam, TQuartDictType], + env_cam, + ) -def produce_env_sim_tquart_gripper_camera_cg( + +def produce_env_sim_tquart_gripper_camera_rel( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id="0" ) -> gym.Env[ArmObsType, TQuartDictType]: - pass + """Environment with tquart, grippter, camera, collision guard and relative limits""" + env_cam = produce_env_sim_tquart_gripper_camera(mjcf_path, urdf_path, cfg, robot_id) + env_cam = RelativeActionSpace(env_cam) + return typing.cast( + gym.Env[ObsArmsGrCam, LimitedCartOrJointContType], + env_cam, + ) -def produce_env_sim_tquart_gripper_camera_rel( + +def produce_env_sim_tquart_gripper_camera_cg( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id="0" ) -> gym.Env[ArmObsType, TQuartDictType]: - pass + """Environment with tquart, grippter, camera, collision guard and relative limits""" + env_sim = produce_env_sim_tquart_gripper_camera(mjcf_path, urdf_path, cfg, robot_id) + env_cam = CollisionGuard.env_from_xml_paths( + env_sim, + mjcf_path, + urdf_path, + gripper=True, + check_home_collision=False, + ) + return typing.cast( + gym.Env[ObsArmsGrCam, TQuartDictType], + env_cam, + ) -# ... + +def produce_env_sim_tquart_gripper_camera_cg_rel( + mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id="0" +) -> gym.Env[ArmObsType, LimitedCartOrJointContType]: + """Environment with tquart, grippter, camera, collision guard and relative limits""" + env_cam = produce_env_sim_tquart_gripper_camera_cg(mjcf_path, urdf_path, cfg, robot_id) + env_cam = RelativeActionSpace(env_cam) + return typing.cast( + gym.Env[ObsArmsGrCam, LimitedCartOrJointContType], + env_cam, + ) def produce_env_hw(): pass + def produce_env_hw_joints(): pass -# ... def produce_env_sim_joints_gripper_camera_cg_rel(): pass # permutations base(sim|hw) x Control(joints|trpy|tquart) x Util(Gripper,Camera,CollisionGuard,RelativeAction|Gripper,Camera,CollisionGuard|Gripper,Camera,RelativeAction) -# 2 + 2x3 + 2x3x3 = 26 permutations \ No newline at end of file +# 2 + 2x3 + 2x3x3 = 26 permutations From dfbf61a0fd0382f8325ed2c5d3e1f1959a30869d Mon Sep 17 00:00:00 2001 From: suman1209 Date: Thu, 11 Jul 2024 17:29:56 +0200 Subject: [PATCH 03/22] feat(type_factories): added sim creation functions for trpy and joints --- python/rcsss/envs/typed_factory.py | 204 +++++++++++++++++++++++------ 1 file changed, 167 insertions(+), 37 deletions(-) diff --git a/python/rcsss/envs/typed_factory.py b/python/rcsss/envs/typed_factory.py index e39c86e0..2bb803c5 100644 --- a/python/rcsss/envs/typed_factory.py +++ b/python/rcsss/envs/typed_factory.py @@ -1,5 +1,6 @@ import typing import rcsss +from rcsss.envs.hw import FR3HW from rcsss.envs.sim import CollisionGuard from rcsss.envs.base import ( CameraSetWrapper, @@ -8,7 +9,10 @@ GripperWrapper, RelativeActionSpace, ObsArmsGrCam, - LimitedCartOrJointContType + LimitedCartOrJointContType, + LimitedTQuartRelDictType, + LimitedTRPYRelDictType, + LimitedJointsRelDictType ) from rcsss import sim from rcsss._core.sim import CameraType, SimCameraSet, SimCameraSetConfig @@ -16,65 +20,59 @@ from rcsss.envs.base import ArmObsType, CameraSetWrapper, CartOrJointContType, ControlMode, FR3Env, JointsDictType, TQuartDictType, TRPYDictType from rcsss.envs.sim import FR3Sim import gymnasium as gym +from rcsss import hw def produce_env_sim( mjcf_path: str, urdf_path: str, control_mode: ControlMode, cfg: sim.FR3Config, robot_id="0" -) -> gym.Env[ArmObsType, CartOrJointContType]: +) -> tuple[gym.Env[ArmObsType, CartOrJointContType], dict[str, typing.Any]]: simulation = sim.Sim(mjcf_path) robot = sim.FR3(simulation, robot_id, urdf_path) robot.set_parameters(cfg) env = FR3Env(robot, control_mode) env_sim = FR3Sim(env, simulation) - return typing.cast(gym.Env[ArmObsType, CartOrJointContType], FR3Sim(env_sim, simulation)) + return typing.cast(gym.Env[ArmObsType, CartOrJointContType], FR3Sim(env_sim, simulation)), {"sim": simulation, + "robot": robot} def produce_env_sim_joints( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id="0" -) -> gym.Env[ArmObsType, JointsDictType]: +) -> tuple[gym.Env[ArmObsType, JointsDictType], dict[str, typing.Any]]: + env, info = produce_env_sim(mjcf_path, urdf_path, control_mode=ControlMode.JOINTS, cfg=cfg, robot_id=robot_id) return typing.cast( gym.Env[ArmObsType, JointsDictType], - produce_env_sim(mjcf_path, urdf_path, control_mode=ControlMode.JOINTS, cfg=cfg, robot_id=robot_id), - ) + env, + ), info def produce_env_sim_trpy( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id="0" -) -> gym.Env[ArmObsType, TRPYDictType]: +) -> tuple[gym.Env[ArmObsType, TRPYDictType], dict[str, typing.Any]]: + env, info = produce_env_sim(mjcf_path, urdf_path, control_mode=ControlMode.JOINTS, cfg=cfg, robot_id=robot_id) return typing.cast( gym.Env[ArmObsType, TRPYDictType], - produce_env_sim(mjcf_path, urdf_path, control_mode=ControlMode.CARTESIAN_TRPY, cfg=cfg, robot_id=robot_id), - ) + env, + ), info def produce_env_sim_tquart( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id="0" -) -> gym.Env[ArmObsType, TQuartDictType]: - return typing.cast( - gym.Env[ArmObsType, TQuartDictType], - produce_env_sim(mjcf_path, urdf_path, control_mode=ControlMode.CARTESIAN_TQuart, cfg=cfg, robot_id=robot_id), - ) +) -> tuple[gym.Env[ArmObsType, TQuartDictType], dict[str, typing.Any]]: + env, info = produce_env_sim(mjcf_path, urdf_path, control_mode=ControlMode.CARTESIAN_TQuart, cfg=cfg, robot_id=robot_id) + return typing.cast(gym.Env[ArmObsType, TQuartDictType], env), info def produce_env_sim_tquart_gripper_camera( - mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id="0" + mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, + cam_cfg: rcsss.camera.sim.SimCameraSetConfig, robot_id="0" ) -> gym.Env[ArmObsType, TQuartDictType]: """Environment with tquart, grippter, camera, collision guard and relative limits""" - simulation = sim.Sim(mjcf_path) - env_sim = produce_env_sim_tquart(mjcf_path, urdf_path, cfg, robot_id) - gripper_cfg = sim.FHConfig() - gripper = sim.FrankaHand(simulation, "0", gripper_cfg) + env_sim, info = produce_env_sim_tquart(mjcf_path, urdf_path, cfg, robot_id) + simulation = info["sim"] + gripper = sim.FrankaHand(info["sim"], "0", gripper_cfg) env_sim = GripperWrapper(env_sim, gripper) - 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=640, resolution_height=480, frame_rate=50) 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) return typing.cast( gym.Env[ObsArmsGrCam, TQuartDictType], @@ -83,22 +81,24 @@ def produce_env_sim_tquart_gripper_camera( def produce_env_sim_tquart_gripper_camera_rel( - mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id="0" + mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, + cam_cfg: rcsss.camera.sim.SimCameraSetConfig, robot_id="0" ) -> gym.Env[ArmObsType, TQuartDictType]: """Environment with tquart, grippter, camera, collision guard and relative limits""" - env_cam = produce_env_sim_tquart_gripper_camera(mjcf_path, urdf_path, cfg, robot_id) + env_cam = produce_env_sim_tquart_gripper_camera(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) env_cam = RelativeActionSpace(env_cam) return typing.cast( - gym.Env[ObsArmsGrCam, LimitedCartOrJointContType], + gym.Env[ObsArmsGrCam, LimitedTQuartRelDictType], env_cam, ) def produce_env_sim_tquart_gripper_camera_cg( - mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id="0" + mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, + cam_cfg: rcsss.camera.sim.SimCameraSetConfig, robot_id="0" ) -> gym.Env[ArmObsType, TQuartDictType]: """Environment with tquart, grippter, camera, collision guard and relative limits""" - env_sim = produce_env_sim_tquart_gripper_camera(mjcf_path, urdf_path, cfg, robot_id) + env_sim = produce_env_sim_tquart_gripper_camera(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) env_cam = CollisionGuard.env_from_xml_paths( env_sim, mjcf_path, @@ -113,19 +113,149 @@ def produce_env_sim_tquart_gripper_camera_cg( def produce_env_sim_tquart_gripper_camera_cg_rel( - mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id="0" + mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, + cam_cfg: rcsss.camera.sim.SimCameraSetConfig, robot_id="0" ) -> gym.Env[ArmObsType, LimitedCartOrJointContType]: """Environment with tquart, grippter, camera, collision guard and relative limits""" - env_cam = produce_env_sim_tquart_gripper_camera_cg(mjcf_path, urdf_path, cfg, robot_id) + env_cam = produce_env_sim_tquart_gripper_camera_cg(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) env_cam = RelativeActionSpace(env_cam) return typing.cast( - gym.Env[ObsArmsGrCam, LimitedCartOrJointContType], + gym.Env[ObsArmsGrCam, LimitedTQuartRelDictType], env_cam, ) -def produce_env_hw(): - pass +def produce_env_sim_trpy_gripper_camera( + mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, + cam_cfg: rcsss.camera.sim.SimCameraSetConfig, robot_id="0" +) -> gym.Env[ArmObsType, TRPYDictType]: + """Environment with tquart, grippter, camera, collision guard and relative limits""" + env_sim, info = produce_env_sim_trpy(mjcf_path, urdf_path, cfg, robot_id) + simulation = info["sim"] + gripper = sim.FrankaHand(info["sim"], "0", gripper_cfg) + env_sim = GripperWrapper(env_sim, gripper) + camera_set = SimCameraSet(simulation, cam_cfg) + env_cam: gym.Env = CameraSetWrapper(env_sim, camera_set) + env_cam = GripperWrapper(env_cam, gripper) + return typing.cast( + gym.Env[ObsArmsGrCam, TRPYDictType], + env_cam, + ) + + +def produce_env_sim_trpy_gripper_camera_rel( + mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, + cam_cfg: rcsss.camera.sim.SimCameraSetConfig, robot_id="0" +) -> gym.Env[ArmObsType, TRPYDictType]: + """Environment with tquart, grippter, camera, collision guard and relative limits""" + env_cam = produce_env_sim_trpy_gripper_camera(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) + env_cam = RelativeActionSpace(env_cam) + return typing.cast( + gym.Env[ObsArmsGrCam, LimitedTRPYRelDictType], + env_cam, + ) + + +def produce_env_sim_trpy_gripper_camera_cg( + mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, + cam_cfg: rcsss.camera.sim.SimCameraSetConfig, robot_id="0" +) -> gym.Env[ArmObsType, TRPYDictType]: + """Environment with tquart, grippter, camera, collision guard and relative limits""" + env_sim = produce_env_sim_trpy_gripper_camera(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) + env_cam = CollisionGuard.env_from_xml_paths( + env_sim, + mjcf_path, + urdf_path, + gripper=True, + check_home_collision=False, + ) + return typing.cast( + gym.Env[ObsArmsGrCam, TRPYDictType], + env_cam, + ) + + +def produce_env_sim_trpy_gripper_camera_cg_rel( + mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, + cam_cfg: rcsss.camera.sim.SimCameraSetConfig, robot_id="0" +) -> gym.Env[ArmObsType, LimitedTRPYRelDictType]: + """Environment with tquart, grippter, camera, collision guard and relative limits""" + env_cam = produce_env_sim_trpy_gripper_camera_cg(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) + env_cam = RelativeActionSpace(env_cam) + return typing.cast( + gym.Env[ObsArmsGrCam, LimitedTRPYRelDictType], + env_cam, + ) + + +def produce_env_sim_joints_gripper_camera( + mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, + cam_cfg: rcsss.camera.sim.SimCameraSetConfig, robot_id="0" +) -> gym.Env[ArmObsType, JointsDictType]: + """Environment with tquart, grippter, camera""" + env_sim, info = produce_env_sim_joints(mjcf_path, urdf_path, cfg, robot_id) + simulation = info["sim"] + gripper = sim.FrankaHand(info["sim"], "0", gripper_cfg) + env_sim = GripperWrapper(env_sim, gripper) + camera_set = SimCameraSet(simulation, cam_cfg) + env_cam: gym.Env = CameraSetWrapper(env_sim, camera_set) + env_cam = GripperWrapper(env_cam, gripper) + return typing.cast( + gym.Env[ObsArmsGrCam, JointsDictType], + env_cam, + ) + + +def produce_env_sim_joints_gripper_camera_rel( + mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, + cam_cfg: rcsss.camera.sim.SimCameraSetConfig, robot_id="0" +) -> gym.Env[ArmObsType, JointsDictType]: + """Environment with tquart, grippter, camera, collision guard and relative limits""" + env_cam = produce_env_sim_joints_gripper_camera(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) + env_cam = RelativeActionSpace(env_cam) + return typing.cast( + gym.Env[ObsArmsGrCam, LimitedJointsRelDictType], + env_cam, + ) + + +def produce_env_sim_joints_gripper_camera_cg( + mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, + cam_cfg: rcsss.camera.sim.SimCameraSetConfig, robot_id="0" +) -> gym.Env[ArmObsType, JointsDictType]: + """Environment with tquart, grippter, camera, collision guard and relative limits""" + env_sim = produce_env_sim_joints_gripper_camera(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) + env_cam = CollisionGuard.env_from_xml_paths( + env_sim, + mjcf_path, + urdf_path, + gripper=True, + check_home_collision=False, + ) + return typing.cast( + gym.Env[ObsArmsGrCam, JointsDictType], + env_cam, + ) + + +def produce_env_sim_joints_gripper_camera_cg_rel( + mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, + cam_cfg: rcsss.camera.sim.SimCameraSetConfig, robot_id="0" +) -> gym.Env[ArmObsType, LimitedJointsRelDictType]: + """Environment with tquart, gripper, camera, collision guard and relative limits""" + env_cam = produce_env_sim_joints_gripper_camera_cg(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) + env_cam = RelativeActionSpace(env_cam) + return typing.cast( + gym.Env[ObsArmsGrCam, LimitedJointsRelDictType], + env_cam, + ) + + +def produce_env_hw(ip: str, urdf_path: str) -> tuple[gym.Env[ArmObsType, CartOrJointContType]]: + robot = hw.FR3(ip, urdf_path) + env = FR3Env(robot, ControlMode.JOINTS) + env_hw = FR3HW(env) + return typing.cast(gym.Env[ArmObsType, CartOrJointContType], env_hw) def produce_env_hw_joints(): From 19a6b29bbf5d630113c5abce2f1d799db2aa0798 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 18 Jul 2024 11:41:13 +0200 Subject: [PATCH 04/22] fix: typed factories Fixed type hints, env naming and upsis --- python/rcsss/envs/typed_factory.py | 34 +++++++++++++++--------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/python/rcsss/envs/typed_factory.py b/python/rcsss/envs/typed_factory.py index 2bb803c5..97b64fe4 100644 --- a/python/rcsss/envs/typed_factory.py +++ b/python/rcsss/envs/typed_factory.py @@ -1,5 +1,7 @@ import typing import rcsss +from rcsss._core.sim import CameraType +from rcsss.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig from rcsss.envs.hw import FR3HW from rcsss.envs.sim import CollisionGuard from rcsss.envs.base import ( @@ -15,8 +17,6 @@ LimitedJointsRelDictType ) from rcsss import sim -from rcsss._core.sim import CameraType, SimCameraSet, SimCameraSetConfig -from rcsss.camera.sim import SimCameraConfig from rcsss.envs.base import ArmObsType, CameraSetWrapper, CartOrJointContType, ControlMode, FR3Env, JointsDictType, TQuartDictType, TRPYDictType from rcsss.envs.sim import FR3Sim import gymnasium as gym @@ -24,7 +24,7 @@ def produce_env_sim( - mjcf_path: str, urdf_path: str, control_mode: ControlMode, cfg: sim.FR3Config, robot_id="0" + mjcf_path: str, urdf_path: str, control_mode: ControlMode, cfg: sim.FR3Config, robot_id: str = "0" ) -> tuple[gym.Env[ArmObsType, CartOrJointContType], dict[str, typing.Any]]: simulation = sim.Sim(mjcf_path) robot = sim.FR3(simulation, robot_id, urdf_path) @@ -36,7 +36,7 @@ def produce_env_sim( def produce_env_sim_joints( - mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id="0" + mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id: str = "0" ) -> tuple[gym.Env[ArmObsType, JointsDictType], dict[str, typing.Any]]: env, info = produce_env_sim(mjcf_path, urdf_path, control_mode=ControlMode.JOINTS, cfg=cfg, robot_id=robot_id) return typing.cast( @@ -46,9 +46,9 @@ def produce_env_sim_joints( def produce_env_sim_trpy( - mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id="0" + mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id: str = "0" ) -> tuple[gym.Env[ArmObsType, TRPYDictType], dict[str, typing.Any]]: - env, info = produce_env_sim(mjcf_path, urdf_path, control_mode=ControlMode.JOINTS, cfg=cfg, robot_id=robot_id) + env, info = produce_env_sim(mjcf_path, urdf_path, control_mode=ControlMode.CARTESIAN_TRPY, cfg=cfg, robot_id=robot_id) return typing.cast( gym.Env[ArmObsType, TRPYDictType], env, @@ -56,7 +56,7 @@ def produce_env_sim_trpy( def produce_env_sim_tquart( - mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id="0" + mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id: str = "0" ) -> tuple[gym.Env[ArmObsType, TQuartDictType], dict[str, typing.Any]]: env, info = produce_env_sim(mjcf_path, urdf_path, control_mode=ControlMode.CARTESIAN_TQuart, cfg=cfg, robot_id=robot_id) return typing.cast(gym.Env[ArmObsType, TQuartDictType], env), info @@ -64,34 +64,34 @@ def produce_env_sim_tquart( def produce_env_sim_tquart_gripper_camera( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, - cam_cfg: rcsss.camera.sim.SimCameraSetConfig, robot_id="0" + cam_cfg: SimCameraSetConfig, robot_id: str = "0" ) -> gym.Env[ArmObsType, TQuartDictType]: """Environment with tquart, grippter, camera, collision guard and relative limits""" env_sim, info = produce_env_sim_tquart(mjcf_path, urdf_path, cfg, robot_id) simulation = info["sim"] - gripper = sim.FrankaHand(info["sim"], "0", gripper_cfg) - env_sim = GripperWrapper(env_sim, gripper) + gripper = sim.FrankaHand(simulation, robot_id, gripper_cfg) + env_sim_gripper = GripperWrapper(env_sim, gripper) camera_set = SimCameraSet(simulation, cam_cfg) - env_cam: gym.Env = CameraSetWrapper(env_sim, camera_set) - env_cam = GripperWrapper(env_cam, gripper) + env_sim_gripper_cam: gym.Env = CameraSetWrapper(env_sim_gripper, camera_set) return typing.cast( gym.Env[ObsArmsGrCam, TQuartDictType], - env_cam, + env_sim_gripper_cam, ) def produce_env_sim_tquart_gripper_camera_rel( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, - cam_cfg: rcsss.camera.sim.SimCameraSetConfig, robot_id="0" -) -> gym.Env[ArmObsType, TQuartDictType]: + cam_cfg: SimCameraSetConfig, robot_id="0" +) -> gym.Env[ArmObsType, LimitedTQuartRelDictType]: """Environment with tquart, grippter, camera, collision guard and relative limits""" env_cam = produce_env_sim_tquart_gripper_camera(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) - env_cam = RelativeActionSpace(env_cam) + env_rel = RelativeActionSpace(env_cam) return typing.cast( gym.Env[ObsArmsGrCam, LimitedTQuartRelDictType], - env_cam, + env_rel, ) +# --- fixed until here ---- def produce_env_sim_tquart_gripper_camera_cg( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, From 32520f7576bb279a24ae3267427df339df9162f9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 18 Jul 2024 12:29:22 +0200 Subject: [PATCH 05/22] feat: added example for typed factory --- python/rcsss/envs/typed_factory.py | 31 ++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/python/rcsss/envs/typed_factory.py b/python/rcsss/envs/typed_factory.py index 97b64fe4..93df463e 100644 --- a/python/rcsss/envs/typed_factory.py +++ b/python/rcsss/envs/typed_factory.py @@ -267,3 +267,34 @@ def produce_env_sim_joints_gripper_camera_cg_rel(): # permutations base(sim|hw) x Control(joints|trpy|tquart) x Util(Gripper,Camera,CollisionGuard,RelativeAction|Gripper,Camera,CollisionGuard|Gripper,Camera,RelativeAction) # 2 + 2x3 + 2x3x3 = 26 permutations +if __name__ == "__main__": + mjcf = "models/mjcf/fr3_modular/scene.xml" + urdf = "models/fr3/urdf/fr3_from_panda.urdf" + robot_id = "0" + + cfg = sim.FR3Config() + cfg.ik_duration_in_milliseconds = 300 + cfg.realtime = False + gripper_cfg = sim.FHConfig() + + 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=640, resolution_height=480, frame_rate=50) + + env = produce_env_sim_tquart_gripper_camera_rel(mjcf_path=mjcf, + urdf_path=urdf, + cfg=cfg, + gripper_cfg=gripper_cfg, + cam_cfg=cam_cfg, + robot_id=robot_id + ) + obs, info = env.reset() + for _ in range(100): + act = env.action_space.sample() + act["tquart"][3:] = [0, 0, 0, 1] + obs, reward, terminated, truncated, info = env.step(act) + if truncated or terminated: + env.reset() + From dfc496598c37609bef1c61f6a6542d457f8eddfa Mon Sep 17 00:00:00 2001 From: suman1209 Date: Thu, 25 Jul 2024 14:29:33 +0200 Subject: [PATCH 06/22] feat(type_factories): added hw env creation functions for trpy,joints and tqwart control modes --- python/rcsss/envs/base.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/python/rcsss/envs/base.py b/python/rcsss/envs/base.py index ff354217..181c460c 100644 --- a/python/rcsss/envs/base.py +++ b/python/rcsss/envs/base.py @@ -123,10 +123,18 @@ class ActionsRelative(LimitedJointsRelDictType): pass +class ObsArmsGr(ArmObsType, GripperDictType): + pass + + class ObsArmsGrCam(ArmObsType, GripperDictType, CameraDictType): pass +class ObsArmsGrCamCG(ArmObsType, GripperDictType, CameraDictType): + pass + + class ControlMode(Enum): JOINTS = 1 CARTESIAN_TRPY = 2 From cbfd8ed313b2f1f4d5ca0229a2a5cbaae63e5fd0 Mon Sep 17 00:00:00 2001 From: suman1209 Date: Thu, 25 Jul 2024 14:29:43 +0200 Subject: [PATCH 07/22] feat(type_factories): added hw env creation functions for trpy,joints and tqwart control modes --- python/rcsss/envs/typed_factory.py | 398 ++++++++++++++++++++++------- 1 file changed, 300 insertions(+), 98 deletions(-) diff --git a/python/rcsss/envs/typed_factory.py b/python/rcsss/envs/typed_factory.py index 93df463e..4b0e17d9 100644 --- a/python/rcsss/envs/typed_factory.py +++ b/python/rcsss/envs/typed_factory.py @@ -1,13 +1,10 @@ import typing -import rcsss from rcsss._core.sim import CameraType from rcsss.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig +from rcsss.camera.realsense import RealSenseCameraSet, RealSenseSetConfig from rcsss.envs.hw import FR3HW from rcsss.envs.sim import CollisionGuard from rcsss.envs.base import ( - CameraSetWrapper, - ControlMode, - FR3Env, GripperWrapper, RelativeActionSpace, ObsArmsGrCam, @@ -17,10 +14,13 @@ LimitedJointsRelDictType ) from rcsss import sim -from rcsss.envs.base import ArmObsType, CameraSetWrapper, CartOrJointContType, ControlMode, FR3Env, JointsDictType, TQuartDictType, TRPYDictType +from rcsss.envs.base import (ArmObsType, CameraSetWrapper, CartOrJointContType, ControlMode, FR3Env, + JointsDictType, TQuartDictType, TRPYDictType, ObsArmsGr) from rcsss.envs.sim import FR3Sim import gymnasium as gym from rcsss import hw +from rcsss.config import read_config_yaml +from rcsss.desk import Desk def produce_env_sim( @@ -48,7 +48,8 @@ def produce_env_sim_joints( def produce_env_sim_trpy( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id: str = "0" ) -> tuple[gym.Env[ArmObsType, TRPYDictType], dict[str, typing.Any]]: - env, info = produce_env_sim(mjcf_path, urdf_path, control_mode=ControlMode.CARTESIAN_TRPY, cfg=cfg, robot_id=robot_id) + env, info = produce_env_sim(mjcf_path, urdf_path, control_mode=ControlMode.CARTESIAN_TRPY, + cfg=cfg, robot_id=robot_id) return typing.cast( gym.Env[ArmObsType, TRPYDictType], env, @@ -58,7 +59,8 @@ def produce_env_sim_trpy( def produce_env_sim_tquart( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id: str = "0" ) -> tuple[gym.Env[ArmObsType, TQuartDictType], dict[str, typing.Any]]: - env, info = produce_env_sim(mjcf_path, urdf_path, control_mode=ControlMode.CARTESIAN_TQuart, cfg=cfg, robot_id=robot_id) + env, info = produce_env_sim(mjcf_path, urdf_path, control_mode=ControlMode.CARTESIAN_TQuart, + cfg=cfg, robot_id=robot_id) return typing.cast(gym.Env[ArmObsType, TQuartDictType], env), info @@ -66,7 +68,7 @@ def produce_env_sim_tquart_gripper_camera( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, cam_cfg: SimCameraSetConfig, robot_id: str = "0" ) -> gym.Env[ArmObsType, TQuartDictType]: - """Environment with tquart, grippter, camera, collision guard and relative limits""" + """Environment with tquart, gripper, camera, collision guard and relative limits""" env_sim, info = produce_env_sim_tquart(mjcf_path, urdf_path, cfg, robot_id) simulation = info["sim"] gripper = sim.FrankaHand(simulation, robot_id, gripper_cfg) @@ -81,9 +83,9 @@ def produce_env_sim_tquart_gripper_camera( def produce_env_sim_tquart_gripper_camera_rel( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, - cam_cfg: SimCameraSetConfig, robot_id="0" + cam_cfg: SimCameraSetConfig, robot_id: str = "0" ) -> gym.Env[ArmObsType, LimitedTQuartRelDictType]: - """Environment with tquart, grippter, camera, collision guard and relative limits""" + """Environment with tquart, gripper, camera, collision guard and relative limits""" env_cam = produce_env_sim_tquart_gripper_camera(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) env_rel = RelativeActionSpace(env_cam) return typing.cast( @@ -91,16 +93,16 @@ def produce_env_sim_tquart_gripper_camera_rel( env_rel, ) -# --- fixed until here ---- def produce_env_sim_tquart_gripper_camera_cg( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, - cam_cfg: rcsss.camera.sim.SimCameraSetConfig, robot_id="0" + cam_cfg: SimCameraSetConfig, robot_id: str = "0" ) -> gym.Env[ArmObsType, TQuartDictType]: - """Environment with tquart, grippter, camera, collision guard and relative limits""" - env_sim = produce_env_sim_tquart_gripper_camera(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) - env_cam = CollisionGuard.env_from_xml_paths( - env_sim, + """Environment with tquart, gripper, camera, collision guard and relative limits""" + env_sim_tquart_gripper_cam = produce_env_sim_tquart_gripper_camera(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, @@ -108,62 +110,64 @@ def produce_env_sim_tquart_gripper_camera_cg( ) return typing.cast( gym.Env[ObsArmsGrCam, TQuartDictType], - env_cam, + env_sim_tquart_gripper_cam_cg, ) def produce_env_sim_tquart_gripper_camera_cg_rel( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, - cam_cfg: rcsss.camera.sim.SimCameraSetConfig, robot_id="0" + cam_cfg: SimCameraSetConfig, robot_id: str = "0" ) -> gym.Env[ArmObsType, LimitedCartOrJointContType]: - """Environment with tquart, grippter, camera, collision guard and relative limits""" - env_cam = produce_env_sim_tquart_gripper_camera_cg(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) - env_cam = RelativeActionSpace(env_cam) + """Environment with tquart, gripper, camera, collision guard and relative limits""" + env_sim_tquart_gripper_cam = produce_env_sim_tquart_gripper_camera_cg(mjcf_path, urdf_path, cfg, + gripper_cfg, cam_cfg, robot_id) + env_sim_tquart_gripper_cam_cg = RelativeActionSpace(env_sim_tquart_gripper_cam) return typing.cast( gym.Env[ObsArmsGrCam, LimitedTQuartRelDictType], - env_cam, + env_sim_tquart_gripper_cam_cg, ) def produce_env_sim_trpy_gripper_camera( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, - cam_cfg: rcsss.camera.sim.SimCameraSetConfig, robot_id="0" + cam_cfg: SimCameraSetConfig, robot_id: str = "0" ) -> gym.Env[ArmObsType, TRPYDictType]: - """Environment with tquart, grippter, camera, collision guard and relative limits""" - env_sim, info = produce_env_sim_trpy(mjcf_path, urdf_path, cfg, robot_id) + """Environment with trpy, gripper, camera, collision guard and relative limits""" + env_sim_trpy, info = produce_env_sim_trpy(mjcf_path, urdf_path, cfg, robot_id) simulation = info["sim"] - gripper = sim.FrankaHand(info["sim"], "0", gripper_cfg) - env_sim = GripperWrapper(env_sim, gripper) + gripper = sim.FrankaHand(simulation, robot_id, gripper_cfg) + env_sim_trpy_gripper = GripperWrapper(env_sim_trpy, gripper) camera_set = SimCameraSet(simulation, cam_cfg) - env_cam: gym.Env = CameraSetWrapper(env_sim, camera_set) - env_cam = GripperWrapper(env_cam, gripper) + env_sim_trpy_gripper_cam: gym.Env = CameraSetWrapper(env_sim_trpy_gripper, camera_set) return typing.cast( gym.Env[ObsArmsGrCam, TRPYDictType], - env_cam, + env_sim_trpy_gripper_cam, ) def produce_env_sim_trpy_gripper_camera_rel( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, - cam_cfg: rcsss.camera.sim.SimCameraSetConfig, robot_id="0" + cam_cfg: SimCameraSetConfig, robot_id: str = "0" ) -> gym.Env[ArmObsType, TRPYDictType]: - """Environment with tquart, grippter, camera, collision guard and relative limits""" - env_cam = produce_env_sim_trpy_gripper_camera(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) - env_cam = RelativeActionSpace(env_cam) + """Environment with trpy, gripper, camera, collision guard and relative limits""" + env_sim_trpy_gripper_cam = produce_env_sim_trpy_gripper_camera(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, + robot_id) + env_sim_trpy_gripper_cam_rel = RelativeActionSpace(env_sim_trpy_gripper_cam) return typing.cast( gym.Env[ObsArmsGrCam, LimitedTRPYRelDictType], - env_cam, + env_sim_trpy_gripper_cam_rel, ) def produce_env_sim_trpy_gripper_camera_cg( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, - cam_cfg: rcsss.camera.sim.SimCameraSetConfig, robot_id="0" + cam_cfg: SimCameraSetConfig, robot_id: str = "0" ) -> gym.Env[ArmObsType, TRPYDictType]: - """Environment with tquart, grippter, camera, collision guard and relative limits""" - env_sim = produce_env_sim_trpy_gripper_camera(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) - env_cam = CollisionGuard.env_from_xml_paths( - env_sim, + """Environment with trpy, gripper, camera, collision guard and relative limits""" + env_sim_trpy_gripper_cam = produce_env_sim_trpy_gripper_camera(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, @@ -171,62 +175,64 @@ def produce_env_sim_trpy_gripper_camera_cg( ) return typing.cast( gym.Env[ObsArmsGrCam, TRPYDictType], - env_cam, + env_sim_trpy_gripper_cam_cg, ) def produce_env_sim_trpy_gripper_camera_cg_rel( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, - cam_cfg: rcsss.camera.sim.SimCameraSetConfig, robot_id="0" + cam_cfg: SimCameraSetConfig, robot_id: str = "0" ) -> gym.Env[ArmObsType, LimitedTRPYRelDictType]: - """Environment with tquart, grippter, camera, collision guard and relative limits""" - env_cam = produce_env_sim_trpy_gripper_camera_cg(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) - env_cam = RelativeActionSpace(env_cam) + """Environment with trpy, gripper, camera, collision guard and relative limits""" + env_sim_trpy_gripper_camera_cg = produce_env_sim_trpy_gripper_camera_cg(mjcf_path, urdf_path, cfg, + gripper_cfg, cam_cfg, robot_id) + env_sim_trpy_gripper_camera_cg_rel = RelativeActionSpace(env_sim_trpy_gripper_camera_cg) return typing.cast( gym.Env[ObsArmsGrCam, LimitedTRPYRelDictType], - env_cam, + env_sim_trpy_gripper_camera_cg_rel, ) def produce_env_sim_joints_gripper_camera( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, - cam_cfg: rcsss.camera.sim.SimCameraSetConfig, robot_id="0" + cam_cfg: SimCameraSetConfig, robot_id: str = "0" ) -> gym.Env[ArmObsType, JointsDictType]: - """Environment with tquart, grippter, camera""" - env_sim, info = produce_env_sim_joints(mjcf_path, urdf_path, cfg, robot_id) + """Environment with joints, gripper, camera""" + env_sim_joints, info = produce_env_sim_joints(mjcf_path, urdf_path, cfg, robot_id) simulation = info["sim"] - gripper = sim.FrankaHand(info["sim"], "0", gripper_cfg) - env_sim = GripperWrapper(env_sim, gripper) + gripper = sim.FrankaHand(simulation, robot_id, gripper_cfg) + env_sim_joints = GripperWrapper(env_sim_joints, gripper) camera_set = SimCameraSet(simulation, cam_cfg) - env_cam: gym.Env = CameraSetWrapper(env_sim, camera_set) - env_cam = GripperWrapper(env_cam, gripper) + env_sim_joints_cam: gym.Env = CameraSetWrapper(env_sim_joints, camera_set) return typing.cast( gym.Env[ObsArmsGrCam, JointsDictType], - env_cam, + env_sim_joints_cam, ) def produce_env_sim_joints_gripper_camera_rel( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, - cam_cfg: rcsss.camera.sim.SimCameraSetConfig, robot_id="0" + cam_cfg: SimCameraSetConfig, robot_id: str = "0" ) -> gym.Env[ArmObsType, JointsDictType]: - """Environment with tquart, grippter, camera, collision guard and relative limits""" - env_cam = produce_env_sim_joints_gripper_camera(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) - env_cam = RelativeActionSpace(env_cam) + """Environment with joints, gripper, camera, collision guard and relative limits""" + env_sim_joints_cam = produce_env_sim_joints_gripper_camera(mjcf_path, urdf_path, cfg, + gripper_cfg, cam_cfg, robot_id) + env_sim_joints_cam_rel = RelativeActionSpace(env_sim_joints_cam) return typing.cast( gym.Env[ObsArmsGrCam, LimitedJointsRelDictType], - env_cam, + env_sim_joints_cam_rel, ) def produce_env_sim_joints_gripper_camera_cg( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, - cam_cfg: rcsss.camera.sim.SimCameraSetConfig, robot_id="0" + cam_cfg: SimCameraSetConfig, robot_id: str = "0" ) -> gym.Env[ArmObsType, JointsDictType]: - """Environment with tquart, grippter, camera, collision guard and relative limits""" - env_sim = produce_env_sim_joints_gripper_camera(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) - env_cam = CollisionGuard.env_from_xml_paths( - env_sim, + """Environment with joints, gripper, camera, collision guard and relative limits""" + env_sim_joints_gripper_cam = produce_env_sim_joints_gripper_camera(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, @@ -234,67 +240,263 @@ def produce_env_sim_joints_gripper_camera_cg( ) return typing.cast( gym.Env[ObsArmsGrCam, JointsDictType], - env_cam, + env_sim_joints_gripper_cam_cg, ) def produce_env_sim_joints_gripper_camera_cg_rel( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, - cam_cfg: rcsss.camera.sim.SimCameraSetConfig, robot_id="0" + cam_cfg: SimCameraSetConfig, robot_id: str = "0" ) -> gym.Env[ArmObsType, LimitedJointsRelDictType]: - """Environment with tquart, gripper, camera, collision guard and relative limits""" - env_cam = produce_env_sim_joints_gripper_camera_cg(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) - env_cam = RelativeActionSpace(env_cam) + """Environment with joints, gripper, camera, collision guard and relative limits""" + env_sim_joints_gripper_cam_cg = produce_env_sim_joints_gripper_camera_cg(mjcf_path, urdf_path, cfg, gripper_cfg, + cam_cfg, robot_id) + env_sim_joints_gripper_cam_cg_rel = RelativeActionSpace(env_sim_joints_gripper_cam_cg) return typing.cast( gym.Env[ObsArmsGrCam, LimitedJointsRelDictType], - env_cam, + env_sim_joints_gripper_cam_cg_rel, ) -def produce_env_hw(ip: str, urdf_path: str) -> tuple[gym.Env[ArmObsType, CartOrJointContType]]: - robot = hw.FR3(ip, urdf_path) - env = FR3Env(robot, ControlMode.JOINTS) - env_hw = FR3HW(env) +def produce_env_hw(ip: str, urdf_path: str, control_mode: ControlMode, cfg_path: str)\ + -> gym.Env[ArmObsType, CartOrJointContType]: + cfg = read_config_yaml(cfg_path) + with Desk(ip, cfg.hw.username, cfg.hw.password) as d: + d.unlock() + d.activate_fci() + robot = hw.FR3(ip, urdf_path) + env = FR3Env(robot, control_mode) + env_hw = FR3HW(env) + return typing.cast(gym.Env[ArmObsType, CartOrJointContType], env_hw) -def produce_env_hw_joints(): - pass +def produce_env_hw_joints(ip: str, urdf_path: str, control_mode: ControlMode.JOINTS, cfg_path: str)\ + -> gym.Env[ArmObsType, JointsDictType]: + env_hw_joints = produce_env_hw(ip, urdf_path, control_mode, cfg_path) + return typing.cast(gym.Env[ArmObsType, JointsDictType], env_hw_joints) + + +def produce_env_hw_joints_gripper(ip: str, urdf_path: str, control_mode: ControlMode, + gripper_cfg: hw.FHConfig, cfg_path: str) -> ( + gym.Env)[ObsArmsGr, JointsDictType]: + env_hw_joints = produce_env_hw(ip, urdf_path, control_mode, cfg_path) + gripper = hw.FrankaHand(ip, gripper_cfg) + env_hw_joints_gripper = GripperWrapper(env_hw_joints, gripper) + return typing.cast(gym.Env[ObsArmsGr, JointsDictType], env_hw_joints_gripper) + + +def produce_env_hw_joints_gripper_camera(ip: str, urdf_path: str, control_mode: ControlMode, gripper_cfg: sim.FHConfig, + cam_cfg: RealSenseSetConfig, cfg_path: str) \ + -> (gym.Env)[ObsArmsGr, JointsDictType]: + env_hw_joints_gripper = produce_env_hw_joints_gripper(ip, urdf_path, control_mode, gripper_cfg, cfg_path) + camera_set = RealSenseCameraSet(cam_cfg) + env_hw_joints_gripper_cam: gym.Env = CameraSetWrapper(env_hw_joints_gripper, camera_set) + return typing.cast(gym.Env[ObsArmsGrCam, JointsDictType], env_hw_joints_gripper_cam) + + +def produce_env_hw_joints_gripper_camera_rel(ip: str, urdf_path: str, control_mode: ControlMode, + gripper_cfg: sim.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, control_mode, + gripper_cfg, cam_cfg, cfg_path) + env_sim_joints_gripper_cam_rel = RelativeActionSpace(env_hw_joints_gripper_cam) + return typing.cast(gym.Env[ObsArmsGrCam, LimitedJointsRelDictType], env_sim_joints_gripper_cam_rel) + + +def produce_env_hw_joints_gripper_camera_cg(ip: str, urdf_path: str, mjcf_path: str, control_mode: ControlMode, + gripper_cfg: sim.FHConfig, cam_cfg: RealSenseSetConfig, + cfg_path: str)\ + -> gym.Env[ObsArmsGr, JointsDictType]: + env_hw_joints_gripper_cam = produce_env_hw_joints_gripper_camera(ip, urdf_path, control_mode, + 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 + ) + return typing.cast(gym.Env[ObsArmsGrCam, JointsDictType], env_hw_joints_gripper_cam_cg) -def produce_env_sim_joints_gripper_camera_cg_rel(): - pass +def produce_env_hw_joints_gripper_camera_cg_rel(ip: str, urdf_path: str, mjcf_path: str, + control_mode: ControlMode, gripper_cfg: sim.FHConfig, + cam_cfg: RealSenseSetConfig, cfg_path: str)\ + -> gym.Env[ObsArmsGr, LimitedJointsRelDictType]: + env_hw_joints_gripper_cam = produce_env_hw_joints_gripper_camera(ip, urdf_path, control_mode, 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, + ) + return typing.cast(gym.Env[ObsArmsGrCam, LimitedJointsRelDictType], env_hw_joints_gripper_cam_cg) + + +def produce_env_hw_trpy(ip: str, urdf_path: str, control_mode: ControlMode.CARTESIAN_TRPY, cfg_path: str)\ + -> gym.Env[ArmObsType, TRPYDictType]: + env_hw_trpy = produce_env_hw(ip, urdf_path, control_mode, cfg_path) + return typing.cast(gym.Env[ArmObsType, TRPYDictType], env_hw_trpy) + + +def produce_env_hw_trpy_gripper(ip: str, urdf_path: str, control_mode: ControlMode.CARTESIAN_TRPY, + gripper_cfg: hw.FHConfig, cfg_path: str) -> ( + gym.Env)[ObsArmsGr, TRPYDictType]: + env_hw_joints = produce_env_hw(ip, urdf_path, control_mode, cfg_path) + gripper = hw.FrankaHand(ip, gripper_cfg) + env_hw_trpy_gripper = GripperWrapper(env_hw_joints, gripper) + return typing.cast(gym.Env[ObsArmsGr, TRPYDictType], env_hw_trpy_gripper) + + +def produce_env_hw_trpy_gripper_camera(ip: str, urdf_path: str, control_mode: ControlMode, gripper_cfg: sim.FHConfig, + cam_cfg: RealSenseSetConfig, cfg_path: str) \ + -> gym.Env[ObsArmsGr, TRPYDictType]: + env_hw_trpy_gripper = produce_env_hw_trpy_gripper(ip, urdf_path, control_mode, gripper_cfg, cfg_path) + camera_set = RealSenseCameraSet(cam_cfg) + env_hw_trpy_gripper_cam: gym.Env = CameraSetWrapper(env_hw_trpy_gripper, camera_set) + return typing.cast(gym.Env[ObsArmsGrCam, TRPYDictType], env_hw_trpy_gripper_cam) + + +def produce_env_hw_trpy_gripper_camera_rel(ip: str, urdf_path: str, control_mode: ControlMode.CARTESIAN_TRPY, + gripper_cfg: sim.FHConfig, cam_cfg: RealSenseSetConfig, + cfg_path: str) \ + -> gym.Env[ObsArmsGrCam, LimitedTRPYRelDictType]: + env_hw_trpy_gripper_cam = produce_env_hw_trpy_gripper_camera(ip, urdf_path, control_mode, + gripper_cfg, cam_cfg, cfg_path) + env_hw_trpy_gripper_cam_rel = RelativeActionSpace(env_hw_trpy_gripper_cam) + return typing.cast(gym.Env[ObsArmsGrCam, LimitedTRPYRelDictType], env_hw_trpy_gripper_cam_rel) + + +def produce_env_hw_trpy_gripper_camera_cg(ip: str, urdf_path: str, mjcf_path: str, + control_mode: ControlMode.CARTESIAN_TRPY, + gripper_cfg: sim.FHConfig, cam_cfg: RealSenseSetConfig, + cfg_path: str)\ + -> gym.Env[ObsArmsGr, TRPYDictType]: + env_hw_trpy_gripper_cam = produce_env_hw_trpy_gripper_camera(ip, urdf_path, control_mode, + 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 + ) + return typing.cast(gym.Env[ObsArmsGrCam, TRPYDictType], env_hw_trpy_gripper_cam_cg) + + +def produce_env_hw_trpy_gripper_camera_cg_rel(ip: str, urdf_path: str, mjcf_path: str, + control_mode: ControlMode, gripper_cfg: sim.FHConfig, + cam_cfg: RealSenseSetConfig, cfg_path: str)\ + -> gym.Env[ObsArmsGr, LimitedTRPYRelDictType]: + env_hw_trpy_gripper_cam = produce_env_hw_trpy_gripper_camera(ip, urdf_path, control_mode, 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, + ) + return typing.cast(gym.Env[ObsArmsGrCam, LimitedTRPYRelDictType], env_hw_joints_gripper_cam_cg) + + +def produce_env_hw_tquart(ip: str, urdf_path: str, control_mode: ControlMode.CARTESIAN_TQuart, cfg_path: str)\ + -> gym.Env[ArmObsType, TQuartDictType]: + env_hw_tquart = produce_env_hw(ip, urdf_path, control_mode, cfg_path) + return typing.cast(gym.Env[ArmObsType, TQuartDictType], env_hw_tquart) + + +def produce_env_hw_tquart_gripper(ip: str, urdf_path: str, control_mode: ControlMode.CARTESIAN_TQuart, + gripper_cfg: hw.FHConfig, cfg_path: str) -> ( + gym.Env)[ObsArmsGr, TQuartDictType]: + env_hw_tquart = produce_env_hw(ip, urdf_path, control_mode, cfg_path) + gripper = hw.FrankaHand(ip, gripper_cfg) + env_hw_tquart_gripper = GripperWrapper(env_hw_tquart, gripper) + return typing.cast(gym.Env[ObsArmsGr, TQuartDictType], env_hw_tquart_gripper) + + +def produce_env_hw_tquart_gripper_camera(ip: str, urdf_path: str, control_mode: ControlMode.CARTESIAN_TQuart, + gripper_cfg: sim.FHConfig, cam_cfg: RealSenseSetConfig, cfg_path: str) \ + -> gym.Env[ObsArmsGr, TQuartDictType]: + env_hw_tquart_gripper = produce_env_hw_tquart_gripper(ip, urdf_path, control_mode, gripper_cfg, cfg_path) + camera_set = RealSenseCameraSet(cam_cfg) + env_hw_tquart_gripper_cam: gym.Env = CameraSetWrapper(env_hw_tquart_gripper, camera_set) + return typing.cast(gym.Env[ObsArmsGrCam, TQuartDictType], env_hw_tquart_gripper_cam) + + +def produce_env_hw_tquart_gripper_camera_rel(ip: str, urdf_path: str, control_mode: ControlMode.CARTESIAN_TQuart, + gripper_cfg: sim.FHConfig, cam_cfg: RealSenseSetConfig, + cfg_path: str) \ + -> gym.Env[ObsArmsGrCam, LimitedTQuartRelDictType]: + env_hw_tquart_gripper_cam = produce_env_hw_tquart_gripper_camera(ip, urdf_path, control_mode, + gripper_cfg, cam_cfg, cfg_path) + env_sim_tquart_gripper_cam_rel = RelativeActionSpace(env_hw_tquart_gripper_cam) + return typing.cast(gym.Env[ObsArmsGrCam, LimitedTQuartRelDictType], env_sim_tquart_gripper_cam_rel) + + +def produce_env_hw_tquart_gripper_camera_cg(ip: str, urdf_path: str, mjcf_path: str, + control_mode: ControlMode.CARTESIAN_TQuart, + gripper_cfg: sim.FHConfig, cam_cfg: RealSenseSetConfig, + cfg_path: str)\ + -> gym.Env[ObsArmsGr, TQuartDictType]: + env_hw_tquart_gripper_cam = produce_env_hw_tquart_gripper_camera(ip, urdf_path, control_mode, + 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 + ) + return typing.cast(gym.Env[ObsArmsGrCam, TQuartDictType], env_hw_tquart_gripper_cam_cg) + + +def produce_env_hw_tquart_gripper_camera_cg_rel(ip: str, urdf_path: str, mjcf_path: str, + control_mode: ControlMode.CARTESIAN_TQuart, gripper_cfg: sim.FHConfig, + cam_cfg: RealSenseSetConfig, cfg_path: str)\ + -> gym.Env[ObsArmsGr, LimitedTQuartRelDictType]: + env_hw_tquart_gripper_cam = produce_env_hw_tquart_gripper_camera(ip, urdf_path, control_mode, 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, + ) + return typing.cast(gym.Env[ObsArmsGrCam, LimitedTQuartRelDictType], env_hw_tquart_gripper_cam_cg) + -# permutations base(sim|hw) x Control(joints|trpy|tquart) x Util(Gripper,Camera,CollisionGuard,RelativeAction|Gripper,Camera,CollisionGuard|Gripper,Camera,RelativeAction) -# 2 + 2x3 + 2x3x3 = 26 permutations if __name__ == "__main__": mjcf = "models/mjcf/fr3_modular/scene.xml" urdf = "models/fr3/urdf/fr3_from_panda.urdf" - robot_id = "0" + robot_id_ = "0" - cfg = sim.FR3Config() - cfg.ik_duration_in_milliseconds = 300 - cfg.realtime = False + cfg_ = sim.FR3Config() + cfg_.ik_duration_in_milliseconds = 300 + cfg_.realtime = False gripper_cfg = sim.FHConfig() 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=640, resolution_height=480, frame_rate=50) - - env = produce_env_sim_tquart_gripper_camera_rel(mjcf_path=mjcf, - urdf_path=urdf, - cfg=cfg, - gripper_cfg=gripper_cfg, - cam_cfg=cam_cfg, - robot_id=robot_id - ) - obs, info = env.reset() + cam_cfg_ = SimCameraSetConfig(cameras=cameras, resolution_width=640, resolution_height=480, frame_rate=50) + + env_ = produce_env_sim_tquart_gripper_camera_rel(mjcf_path=mjcf, + urdf_path=urdf, + cfg=cfg_, + gripper_cfg=gripper_cfg, + cam_cfg=cam_cfg_, + robot_id=robot_id_) + obs_, info_ = env_.reset() for _ in range(100): - act = env.action_space.sample() + act = env_.action_space.sample() act["tquart"][3:] = [0, 0, 0, 1] - obs, reward, terminated, truncated, info = env.step(act) + obs, reward, terminated, truncated, info = env_.step(act) if truncated or terminated: - env.reset() - + env_.reset() From e3ea95f3143140230fb8b438321729d29adad4c3 Mon Sep 17 00:00:00 2001 From: suman1209 Date: Thu, 25 Jul 2024 15:09:04 +0200 Subject: [PATCH 08/22] fix(type_factories): pylint fixes --- python/rcsss/envs/typed_factory.py | 147 ++++++++++++++--------------- 1 file changed, 69 insertions(+), 78 deletions(-) diff --git a/python/rcsss/envs/typed_factory.py b/python/rcsss/envs/typed_factory.py index 4b0e17d9..f3c05698 100644 --- a/python/rcsss/envs/typed_factory.py +++ b/python/rcsss/envs/typed_factory.py @@ -3,12 +3,12 @@ from rcsss.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig from rcsss.camera.realsense import RealSenseCameraSet, RealSenseSetConfig from rcsss.envs.hw import FR3HW +from rcsss._core.hw import FHConfig from rcsss.envs.sim import CollisionGuard from rcsss.envs.base import ( GripperWrapper, RelativeActionSpace, ObsArmsGrCam, - LimitedCartOrJointContType, LimitedTQuartRelDictType, LimitedTRPYRelDictType, LimitedJointsRelDictType @@ -67,7 +67,7 @@ def produce_env_sim_tquart( def produce_env_sim_tquart_gripper_camera( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, cam_cfg: SimCameraSetConfig, robot_id: str = "0" -) -> gym.Env[ArmObsType, TQuartDictType]: +) -> gym.Env[ObsArmsGrCam, TQuartDictType]: """Environment with tquart, gripper, camera, collision guard and relative limits""" env_sim, info = produce_env_sim_tquart(mjcf_path, urdf_path, cfg, robot_id) simulation = info["sim"] @@ -84,7 +84,7 @@ def produce_env_sim_tquart_gripper_camera( def produce_env_sim_tquart_gripper_camera_rel( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, cam_cfg: SimCameraSetConfig, robot_id: str = "0" -) -> gym.Env[ArmObsType, LimitedTQuartRelDictType]: +) -> gym.Env[ObsArmsGrCam, LimitedTQuartRelDictType]: """Environment with tquart, gripper, camera, collision guard and relative limits""" env_cam = produce_env_sim_tquart_gripper_camera(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) env_rel = RelativeActionSpace(env_cam) @@ -97,7 +97,7 @@ def produce_env_sim_tquart_gripper_camera_rel( def produce_env_sim_tquart_gripper_camera_cg( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, cam_cfg: SimCameraSetConfig, robot_id: str = "0" -) -> gym.Env[ArmObsType, TQuartDictType]: +) -> gym.Env[ObsArmsGrCam, TQuartDictType]: """Environment with tquart, gripper, camera, collision guard and relative limits""" env_sim_tquart_gripper_cam = produce_env_sim_tquart_gripper_camera(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) @@ -117,7 +117,7 @@ def produce_env_sim_tquart_gripper_camera_cg( def produce_env_sim_tquart_gripper_camera_cg_rel( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, cam_cfg: SimCameraSetConfig, robot_id: str = "0" -) -> gym.Env[ArmObsType, LimitedCartOrJointContType]: +) -> gym.Env[ObsArmsGrCam, LimitedTQuartRelDictType]: """Environment with tquart, gripper, camera, collision guard and relative limits""" env_sim_tquart_gripper_cam = produce_env_sim_tquart_gripper_camera_cg(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) @@ -131,7 +131,7 @@ def produce_env_sim_tquart_gripper_camera_cg_rel( def produce_env_sim_trpy_gripper_camera( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, cam_cfg: SimCameraSetConfig, robot_id: str = "0" -) -> gym.Env[ArmObsType, TRPYDictType]: +) -> gym.Env[ObsArmsGrCam, TRPYDictType]: """Environment with trpy, gripper, camera, collision guard and relative limits""" env_sim_trpy, info = produce_env_sim_trpy(mjcf_path, urdf_path, cfg, robot_id) simulation = info["sim"] @@ -148,7 +148,7 @@ def produce_env_sim_trpy_gripper_camera( def produce_env_sim_trpy_gripper_camera_rel( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, cam_cfg: SimCameraSetConfig, robot_id: str = "0" -) -> gym.Env[ArmObsType, TRPYDictType]: +) -> gym.Env[ObsArmsGrCam, TRPYDictType]: """Environment with trpy, gripper, camera, collision guard and relative limits""" env_sim_trpy_gripper_cam = produce_env_sim_trpy_gripper_camera(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) @@ -162,7 +162,7 @@ def produce_env_sim_trpy_gripper_camera_rel( def produce_env_sim_trpy_gripper_camera_cg( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, cam_cfg: SimCameraSetConfig, robot_id: str = "0" -) -> gym.Env[ArmObsType, TRPYDictType]: +) -> gym.Env[ObsArmsGrCam, TRPYDictType]: """Environment with trpy, gripper, camera, collision guard and relative limits""" env_sim_trpy_gripper_cam = produce_env_sim_trpy_gripper_camera(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) @@ -182,7 +182,7 @@ def produce_env_sim_trpy_gripper_camera_cg( def produce_env_sim_trpy_gripper_camera_cg_rel( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, cam_cfg: SimCameraSetConfig, robot_id: str = "0" -) -> gym.Env[ArmObsType, LimitedTRPYRelDictType]: +) -> gym.Env[ObsArmsGrCam, LimitedTRPYRelDictType]: """Environment with trpy, gripper, camera, collision guard and relative limits""" env_sim_trpy_gripper_camera_cg = produce_env_sim_trpy_gripper_camera_cg(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) @@ -196,24 +196,24 @@ def produce_env_sim_trpy_gripper_camera_cg_rel( def produce_env_sim_joints_gripper_camera( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, cam_cfg: SimCameraSetConfig, robot_id: str = "0" -) -> gym.Env[ArmObsType, JointsDictType]: +) -> gym.Env[ObsArmsGrCam, JointsDictType]: """Environment with joints, gripper, camera""" env_sim_joints, info = produce_env_sim_joints(mjcf_path, urdf_path, cfg, robot_id) simulation = info["sim"] gripper = sim.FrankaHand(simulation, robot_id, gripper_cfg) - env_sim_joints = GripperWrapper(env_sim_joints, gripper) + env_sim_joints_gripper = GripperWrapper(env_sim_joints, gripper) camera_set = SimCameraSet(simulation, cam_cfg) - env_sim_joints_cam: gym.Env = CameraSetWrapper(env_sim_joints, camera_set) + env_sim_joints_gripper_cam: gym.Env = CameraSetWrapper(env_sim_joints_gripper, camera_set) return typing.cast( gym.Env[ObsArmsGrCam, JointsDictType], - env_sim_joints_cam, + env_sim_joints_gripper_cam, ) def produce_env_sim_joints_gripper_camera_rel( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, cam_cfg: SimCameraSetConfig, robot_id: str = "0" -) -> gym.Env[ArmObsType, JointsDictType]: +) -> gym.Env[ObsArmsGrCam, JointsDictType]: """Environment with joints, gripper, camera, collision guard and relative limits""" env_sim_joints_cam = produce_env_sim_joints_gripper_camera(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) @@ -227,7 +227,7 @@ def produce_env_sim_joints_gripper_camera_rel( def produce_env_sim_joints_gripper_camera_cg( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, cam_cfg: SimCameraSetConfig, robot_id: str = "0" -) -> gym.Env[ArmObsType, JointsDictType]: +) -> gym.Env[ObsArmsGrCam, JointsDictType]: """Environment with joints, gripper, camera, collision guard and relative limits""" env_sim_joints_gripper_cam = produce_env_sim_joints_gripper_camera(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) @@ -247,7 +247,7 @@ def produce_env_sim_joints_gripper_camera_cg( def produce_env_sim_joints_gripper_camera_cg_rel( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, cam_cfg: SimCameraSetConfig, robot_id: str = "0" -) -> gym.Env[ArmObsType, LimitedJointsRelDictType]: +) -> gym.Env[ObsArmsGrCam, LimitedJointsRelDictType]: """Environment with joints, gripper, camera, collision guard and relative limits""" env_sim_joints_gripper_cam_cg = produce_env_sim_joints_gripper_camera_cg(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) @@ -271,45 +271,45 @@ def produce_env_hw(ip: str, urdf_path: str, control_mode: ControlMode, cfg_path: return typing.cast(gym.Env[ArmObsType, CartOrJointContType], env_hw) -def produce_env_hw_joints(ip: str, urdf_path: str, control_mode: ControlMode.JOINTS, cfg_path: str)\ +def produce_env_hw_joints(ip: str, urdf_path: str, cfg_path: str)\ -> gym.Env[ArmObsType, JointsDictType]: - env_hw_joints = produce_env_hw(ip, urdf_path, control_mode, cfg_path) + env_hw_joints = produce_env_hw(ip, urdf_path, ControlMode.JOINTS, cfg_path) return typing.cast(gym.Env[ArmObsType, JointsDictType], env_hw_joints) -def produce_env_hw_joints_gripper(ip: str, urdf_path: str, control_mode: ControlMode, - gripper_cfg: hw.FHConfig, cfg_path: str) -> ( +def produce_env_hw_joints_gripper(ip: str, urdf_path: str, + gripper_cfg: FHConfig, cfg_path: str) -> ( gym.Env)[ObsArmsGr, JointsDictType]: - env_hw_joints = produce_env_hw(ip, urdf_path, control_mode, cfg_path) + env_hw_joints = produce_env_hw_joints(ip, urdf_path, cfg_path) gripper = hw.FrankaHand(ip, gripper_cfg) env_hw_joints_gripper = GripperWrapper(env_hw_joints, gripper) return typing.cast(gym.Env[ObsArmsGr, JointsDictType], env_hw_joints_gripper) -def produce_env_hw_joints_gripper_camera(ip: str, urdf_path: str, control_mode: ControlMode, gripper_cfg: sim.FHConfig, +def produce_env_hw_joints_gripper_camera(ip: str, urdf_path: str, gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, cfg_path: str) \ - -> (gym.Env)[ObsArmsGr, JointsDictType]: - env_hw_joints_gripper = produce_env_hw_joints_gripper(ip, urdf_path, control_mode, gripper_cfg, cfg_path) + -> (gym.Env)[ObsArmsGrCam, JointsDictType]: + env_hw_joints_gripper = produce_env_hw_joints_gripper(ip, urdf_path, gripper_cfg, cfg_path) camera_set = RealSenseCameraSet(cam_cfg) env_hw_joints_gripper_cam: gym.Env = CameraSetWrapper(env_hw_joints_gripper, camera_set) return typing.cast(gym.Env[ObsArmsGrCam, JointsDictType], env_hw_joints_gripper_cam) -def produce_env_hw_joints_gripper_camera_rel(ip: str, urdf_path: str, control_mode: ControlMode, - gripper_cfg: sim.FHConfig, cam_cfg: RealSenseSetConfig, +def produce_env_hw_joints_gripper_camera_rel(ip: str, urdf_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, control_mode, + env_hw_joints_gripper_cam = produce_env_hw_joints_gripper_camera(ip, urdf_path, gripper_cfg, cam_cfg, cfg_path) env_sim_joints_gripper_cam_rel = RelativeActionSpace(env_hw_joints_gripper_cam) return typing.cast(gym.Env[ObsArmsGrCam, LimitedJointsRelDictType], env_sim_joints_gripper_cam_rel) -def produce_env_hw_joints_gripper_camera_cg(ip: str, urdf_path: str, mjcf_path: str, control_mode: ControlMode, - gripper_cfg: sim.FHConfig, cam_cfg: RealSenseSetConfig, +def produce_env_hw_joints_gripper_camera_cg(ip: str, urdf_path: str, mjcf_path: str, + gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, cfg_path: str)\ - -> gym.Env[ObsArmsGr, JointsDictType]: - env_hw_joints_gripper_cam = produce_env_hw_joints_gripper_camera(ip, urdf_path, control_mode, + -> 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, @@ -321,11 +321,10 @@ def produce_env_hw_joints_gripper_camera_cg(ip: str, urdf_path: str, mjcf_path: return typing.cast(gym.Env[ObsArmsGrCam, JointsDictType], env_hw_joints_gripper_cam_cg) -def produce_env_hw_joints_gripper_camera_cg_rel(ip: str, urdf_path: str, mjcf_path: str, - control_mode: ControlMode, gripper_cfg: sim.FHConfig, +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[ObsArmsGr, LimitedJointsRelDictType]: - env_hw_joints_gripper_cam = produce_env_hw_joints_gripper_camera(ip, urdf_path, control_mode, gripper_cfg, + -> 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, @@ -337,47 +336,44 @@ def produce_env_hw_joints_gripper_camera_cg_rel(ip: str, urdf_path: str, mjcf_pa return typing.cast(gym.Env[ObsArmsGrCam, LimitedJointsRelDictType], env_hw_joints_gripper_cam_cg) -def produce_env_hw_trpy(ip: str, urdf_path: str, control_mode: ControlMode.CARTESIAN_TRPY, cfg_path: str)\ +def produce_env_hw_trpy(ip: str, urdf_path: str, cfg_path: str)\ -> gym.Env[ArmObsType, TRPYDictType]: - env_hw_trpy = produce_env_hw(ip, urdf_path, control_mode, cfg_path) + env_hw_trpy = produce_env_hw(ip, urdf_path, ControlMode.CARTESIAN_TRPY, cfg_path) return typing.cast(gym.Env[ArmObsType, TRPYDictType], env_hw_trpy) -def produce_env_hw_trpy_gripper(ip: str, urdf_path: str, control_mode: ControlMode.CARTESIAN_TRPY, - gripper_cfg: hw.FHConfig, cfg_path: str) -> ( +def produce_env_hw_trpy_gripper(ip: str, urdf_path: str, + gripper_cfg: FHConfig, cfg_path: str) -> ( gym.Env)[ObsArmsGr, TRPYDictType]: - env_hw_joints = produce_env_hw(ip, urdf_path, control_mode, cfg_path) + env_hw_joints = produce_env_hw_trpy(ip, urdf_path, cfg_path) gripper = hw.FrankaHand(ip, gripper_cfg) env_hw_trpy_gripper = GripperWrapper(env_hw_joints, gripper) return typing.cast(gym.Env[ObsArmsGr, TRPYDictType], env_hw_trpy_gripper) -def produce_env_hw_trpy_gripper_camera(ip: str, urdf_path: str, control_mode: ControlMode, gripper_cfg: sim.FHConfig, +def produce_env_hw_trpy_gripper_camera(ip: str, urdf_path: str, gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, cfg_path: str) \ - -> gym.Env[ObsArmsGr, TRPYDictType]: - env_hw_trpy_gripper = produce_env_hw_trpy_gripper(ip, urdf_path, control_mode, gripper_cfg, cfg_path) + -> gym.Env[ObsArmsGrCam, TRPYDictType]: + env_hw_trpy_gripper = produce_env_hw_trpy_gripper(ip, urdf_path, gripper_cfg, cfg_path) camera_set = RealSenseCameraSet(cam_cfg) env_hw_trpy_gripper_cam: gym.Env = CameraSetWrapper(env_hw_trpy_gripper, camera_set) return typing.cast(gym.Env[ObsArmsGrCam, TRPYDictType], env_hw_trpy_gripper_cam) -def produce_env_hw_trpy_gripper_camera_rel(ip: str, urdf_path: str, control_mode: ControlMode.CARTESIAN_TRPY, - gripper_cfg: sim.FHConfig, cam_cfg: RealSenseSetConfig, +def produce_env_hw_trpy_gripper_camera_rel(ip: str, urdf_path: str, + gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, cfg_path: str) \ -> gym.Env[ObsArmsGrCam, LimitedTRPYRelDictType]: - env_hw_trpy_gripper_cam = produce_env_hw_trpy_gripper_camera(ip, urdf_path, control_mode, - gripper_cfg, cam_cfg, cfg_path) + 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_rel = RelativeActionSpace(env_hw_trpy_gripper_cam) return typing.cast(gym.Env[ObsArmsGrCam, LimitedTRPYRelDictType], env_hw_trpy_gripper_cam_rel) def produce_env_hw_trpy_gripper_camera_cg(ip: str, urdf_path: str, mjcf_path: str, - control_mode: ControlMode.CARTESIAN_TRPY, - gripper_cfg: sim.FHConfig, cam_cfg: RealSenseSetConfig, + gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, cfg_path: str)\ - -> gym.Env[ObsArmsGr, TRPYDictType]: - env_hw_trpy_gripper_cam = produce_env_hw_trpy_gripper_camera(ip, urdf_path, control_mode, - gripper_cfg, cam_cfg, cfg_path) + -> 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, @@ -388,11 +384,10 @@ def produce_env_hw_trpy_gripper_camera_cg(ip: str, urdf_path: str, mjcf_path: st return typing.cast(gym.Env[ObsArmsGrCam, TRPYDictType], env_hw_trpy_gripper_cam_cg) -def produce_env_hw_trpy_gripper_camera_cg_rel(ip: str, urdf_path: str, mjcf_path: str, - control_mode: ControlMode, gripper_cfg: sim.FHConfig, +def produce_env_hw_trpy_gripper_camera_cg_rel(ip: str, urdf_path: str, mjcf_path: str, gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, cfg_path: str)\ - -> gym.Env[ObsArmsGr, LimitedTRPYRelDictType]: - env_hw_trpy_gripper_cam = produce_env_hw_trpy_gripper_camera(ip, urdf_path, control_mode, gripper_cfg, + -> 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, @@ -404,46 +399,43 @@ def produce_env_hw_trpy_gripper_camera_cg_rel(ip: str, urdf_path: str, mjcf_path return typing.cast(gym.Env[ObsArmsGrCam, LimitedTRPYRelDictType], env_hw_joints_gripper_cam_cg) -def produce_env_hw_tquart(ip: str, urdf_path: str, control_mode: ControlMode.CARTESIAN_TQuart, cfg_path: str)\ +def produce_env_hw_tquart(ip: str, urdf_path: str, cfg_path: str)\ -> gym.Env[ArmObsType, TQuartDictType]: - env_hw_tquart = produce_env_hw(ip, urdf_path, control_mode, cfg_path) + env_hw_tquart = produce_env_hw(ip, urdf_path, ControlMode.CARTESIAN_TQuart, cfg_path) return typing.cast(gym.Env[ArmObsType, TQuartDictType], env_hw_tquart) -def produce_env_hw_tquart_gripper(ip: str, urdf_path: str, control_mode: ControlMode.CARTESIAN_TQuart, - gripper_cfg: hw.FHConfig, cfg_path: str) -> ( +def produce_env_hw_tquart_gripper(ip: str, urdf_path: str, gripper_cfg: FHConfig, cfg_path: str) -> ( gym.Env)[ObsArmsGr, TQuartDictType]: - env_hw_tquart = produce_env_hw(ip, urdf_path, control_mode, cfg_path) + env_hw_tquart = produce_env_hw_tquart(ip, urdf_path, cfg_path) gripper = hw.FrankaHand(ip, gripper_cfg) env_hw_tquart_gripper = GripperWrapper(env_hw_tquart, gripper) return typing.cast(gym.Env[ObsArmsGr, TQuartDictType], env_hw_tquart_gripper) -def produce_env_hw_tquart_gripper_camera(ip: str, urdf_path: str, control_mode: ControlMode.CARTESIAN_TQuart, - gripper_cfg: sim.FHConfig, cam_cfg: RealSenseSetConfig, cfg_path: str) \ - -> gym.Env[ObsArmsGr, TQuartDictType]: - env_hw_tquart_gripper = produce_env_hw_tquart_gripper(ip, urdf_path, control_mode, gripper_cfg, cfg_path) +def produce_env_hw_tquart_gripper_camera(ip: str, urdf_path: str, gripper_cfg: FHConfig, + cam_cfg: RealSenseSetConfig, cfg_path: str) \ + -> gym.Env[ObsArmsGrCam, TQuartDictType]: + env_hw_tquart_gripper = produce_env_hw_tquart_gripper(ip, urdf_path, gripper_cfg, cfg_path) camera_set = RealSenseCameraSet(cam_cfg) env_hw_tquart_gripper_cam: gym.Env = CameraSetWrapper(env_hw_tquart_gripper, camera_set) return typing.cast(gym.Env[ObsArmsGrCam, TQuartDictType], env_hw_tquart_gripper_cam) -def produce_env_hw_tquart_gripper_camera_rel(ip: str, urdf_path: str, control_mode: ControlMode.CARTESIAN_TQuart, - gripper_cfg: sim.FHConfig, cam_cfg: RealSenseSetConfig, +def produce_env_hw_tquart_gripper_camera_rel(ip: str, urdf_path: str, + gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, cfg_path: str) \ -> gym.Env[ObsArmsGrCam, LimitedTQuartRelDictType]: - env_hw_tquart_gripper_cam = produce_env_hw_tquart_gripper_camera(ip, urdf_path, control_mode, + env_hw_tquart_gripper_cam = produce_env_hw_tquart_gripper_camera(ip, urdf_path, gripper_cfg, cam_cfg, cfg_path) env_sim_tquart_gripper_cam_rel = RelativeActionSpace(env_hw_tquart_gripper_cam) return typing.cast(gym.Env[ObsArmsGrCam, LimitedTQuartRelDictType], env_sim_tquart_gripper_cam_rel) def produce_env_hw_tquart_gripper_camera_cg(ip: str, urdf_path: str, mjcf_path: str, - control_mode: ControlMode.CARTESIAN_TQuart, - gripper_cfg: sim.FHConfig, cam_cfg: RealSenseSetConfig, - cfg_path: str)\ - -> gym.Env[ObsArmsGr, TQuartDictType]: - env_hw_tquart_gripper_cam = produce_env_hw_tquart_gripper_camera(ip, urdf_path, control_mode, + gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, cfg_path: str)\ + -> 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, @@ -455,12 +447,11 @@ def produce_env_hw_tquart_gripper_camera_cg(ip: str, urdf_path: str, mjcf_path: return typing.cast(gym.Env[ObsArmsGrCam, TQuartDictType], env_hw_tquart_gripper_cam_cg) -def produce_env_hw_tquart_gripper_camera_cg_rel(ip: str, urdf_path: str, mjcf_path: str, - control_mode: ControlMode.CARTESIAN_TQuart, gripper_cfg: sim.FHConfig, +def produce_env_hw_tquart_gripper_camera_cg_rel(ip: str, urdf_path: str, mjcf_path: str, gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, cfg_path: str)\ - -> gym.Env[ObsArmsGr, LimitedTQuartRelDictType]: - env_hw_tquart_gripper_cam = produce_env_hw_tquart_gripper_camera(ip, urdf_path, control_mode, gripper_cfg, - cam_cfg, cfg_path) + -> gym.Env[ObsArmsGrCam, LimitedTQuartRelDictType]: + env_hw_tquart_gripper_cam = produce_env_hw_tquart_gripper_camera_cg(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, From e25c826d1a6eea2d810724e64150442c5e8575bd Mon Sep 17 00:00:00 2001 From: suman1209 Date: Thu, 25 Jul 2024 17:57:33 +0200 Subject: [PATCH 09/22] feat(integration test type_factories): added initial integration tests --- python/tests/test_sim_envs.py | 98 +++++++++++++++++++++++++++++++++++ 1 file changed, 98 insertions(+) create mode 100644 python/tests/test_sim_envs.py diff --git a/python/tests/test_sim_envs.py b/python/tests/test_sim_envs.py new file mode 100644 index 00000000..fe2b984f --- /dev/null +++ b/python/tests/test_sim_envs.py @@ -0,0 +1,98 @@ +# import pytest +from collections import OrderedDict + +import numpy as np +from rcsss._core.sim import CameraType +from rcsss.camera.sim import SimCameraConfig, SimCameraSetConfig +from rcsss.envs import typed_factory as tf +from rcsss import sim + + +class TestSimEnvs: + mjcf_path = "models/mjcf/fr3_modular/scene.xml" + urdf_path = "models/fr3/urdf/fr3_from_panda.urdf" + + def test_zero_action(self): + """ + Test that a zero action does not change the state significantly + """ + cfg_ = sim.FR3Config() + cfg_.ik_duration_in_milliseconds = 300 + cfg_.realtime = False + env, _ = tf.produce_env_sim_trpy(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg_, robot_id="0") + obs_, info_ = env.reset() + print(f"initial_obs: {obs_}") + zero_action = OrderedDict([('xyzrpy', np.array([1, 1, 1, 0, 0, 0], dtype=np.float32))]) + obs, reward, terminated, truncated, info = env.step(zero_action) + print(f"{obs=}") + print(f"{obs['tquart'] == obs_['tquart']}") + + def test_non_zero_action_trpy(self): + """ + This is for testing that a certain action leads to the expected change in state + """ + cfg_ = sim.FR3Config() + cfg_.ik_duration_in_milliseconds = 300 + cfg_.realtime = False + env, _ = tf.produce_env_sim_trpy(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg_, robot_id="0") + + obs_, info_ = env.reset() + print(f"initial_obs: {obs_}") + # action to be performed + # @todo why does a non zero action not change the observed value + non_zero_action = OrderedDict([('xyzrpy', np.array([1, 1, 1, 0, 0, 0], dtype=np.float32))]) + obs, reward, terminated, truncated, info = env.step(non_zero_action) + print(f"{obs=}") + print(f"{obs['tquart'] == obs_['tquart']}") + + def test_non_zero_action_joints(self): + """ + This is for testing that a certain action leads to the expected change in state + """ + cfg_ = sim.FR3Config() + cfg_.ik_duration_in_milliseconds = 300 + cfg_.realtime = False + env_, _ = tf.produce_env_sim_joints(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg_, robot_id="0") + obs_, info_ = env_.reset() + print(f"initial_obs: {obs_}") + # action to be performed + # @todo why does a non zero action not change the observed value + non_zero_action = OrderedDict([('joints', np.array([1.2854844, 1.0638355, -1.9478809, -1.8864504, + -0.09758008, 4.125365, 0.19993608], dtype=np.float32))]) + + print(f"initial_obs: {obs_}") + obs, reward, terminated, truncated, info = env_.step(non_zero_action) + print(f"{obs=}") + print(f"{obs['tquart'] == obs_['tquart']}") + + def test_collision_guard(self): + """ + Check that an obvious collision is detected by the CollisionGuard + """ + # @todo how to test this functionality? + cfg_ = sim.FR3Config() + cfg_.ik_duration_in_milliseconds = 300 + cfg_.realtime = False + gripper_cfg = sim.FHConfig() + + 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=640, resolution_height=480, frame_rate=50) + + env_ = tf.produce_env_sim_tquart_gripper_camera_cg_rel(mjcf_path=TestSimEnvs.mjcf_path, + urdf_path=TestSimEnvs.urdf_path, + cfg=cfg_, + gripper_cfg=gripper_cfg, + cam_cfg=cam_cfg_, + robot_id='0') + obs_, info_ = env_.reset() + print(f"initial_obs: {obs_}") + for _ in range(2): + act = env_.action_space.sample() + # act["tquart"][3:] = [0, 0, 0, 1] + obs, reward, terminated, truncated, info = env_.step(act) + print(f"{obs=}") + if truncated or terminated: + env_.reset() From 422b70509530493223b4e0d7f5fa26c2a9d2a371 Mon Sep 17 00:00:00 2001 From: suman1209 Date: Thu, 1 Aug 2024 17:53:59 +0200 Subject: [PATCH 10/22] update(integration test type_factories): update to the integration tests --- python/tests/test_sim_envs.py | 77 ++++++++++++++++++----------------- 1 file changed, 39 insertions(+), 38 deletions(-) diff --git a/python/tests/test_sim_envs.py b/python/tests/test_sim_envs.py index fe2b984f..6ef01160 100644 --- a/python/tests/test_sim_envs.py +++ b/python/tests/test_sim_envs.py @@ -1,6 +1,5 @@ -# import pytest +import pytest from collections import OrderedDict - import numpy as np from rcsss._core.sim import CameraType from rcsss.camera.sim import SimCameraConfig, SimCameraSetConfig @@ -12,7 +11,8 @@ class TestSimEnvs: mjcf_path = "models/mjcf/fr3_modular/scene.xml" urdf_path = "models/fr3/urdf/fr3_from_panda.urdf" - def test_zero_action(self): + @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") + def test_zero_action_trpy(self): """ Test that a zero action does not change the state significantly """ @@ -20,31 +20,38 @@ def test_zero_action(self): cfg_.ik_duration_in_milliseconds = 300 cfg_.realtime = False env, _ = tf.produce_env_sim_trpy(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg_, robot_id="0") - obs_, info_ = env.reset() - print(f"initial_obs: {obs_}") - zero_action = OrderedDict([('xyzrpy', np.array([1, 1, 1, 0, 0, 0], dtype=np.float32))]) + obs_initial, info_ = env.reset() + print(f"initial_obs: {obs_initial}") + print(f"info: {info_}") + zero_action = OrderedDict([('xyzrpy', np.array([0, 0, 0, 0, 0, 0], dtype=np.float32))]) + print(f"zero_action: {zero_action}") + # TODO, there seems to be a bug, with two steps of this zero action, the observation changes obs, reward, terminated, truncated, info = env.step(zero_action) print(f"{obs=}") - print(f"{obs['tquart'] == obs_['tquart']}") + assert np.array_equal(obs['tquart'], obs_initial['tquart']) + @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") def test_non_zero_action_trpy(self): """ This is for testing that a certain action leads to the expected change in state """ + # env creation cfg_ = sim.FR3Config() cfg_.ik_duration_in_milliseconds = 300 cfg_.realtime = False env, _ = tf.produce_env_sim_trpy(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg_, robot_id="0") + obs_initial, info_ = env.reset() - obs_, info_ = env.reset() - print(f"initial_obs: {obs_}") + print(f"initial_obs: {obs_initial}") # action to be performed - # @todo why does a non zero action not change the observed value - non_zero_action = OrderedDict([('xyzrpy', np.array([1, 1, 1, 0, 0, 0], dtype=np.float32))]) + non_zero_action = env.action_space.sample() + print(f"non_zero_action: {non_zero_action}") + # TODO, there seems to be a bug, it is required to step two times to see an effect obs, reward, terminated, truncated, info = env.step(non_zero_action) print(f"{obs=}") - print(f"{obs['tquart'] == obs_['tquart']}") + assert not np.array_equal(obs['tquart'], obs_initial['tquart']) + @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") def test_non_zero_action_joints(self): """ This is for testing that a certain action leads to the expected change in state @@ -53,46 +60,40 @@ def test_non_zero_action_joints(self): cfg_.ik_duration_in_milliseconds = 300 cfg_.realtime = False env_, _ = tf.produce_env_sim_joints(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg_, robot_id="0") - obs_, info_ = env_.reset() - print(f"initial_obs: {obs_}") + obs_initial, info_ = env_.reset() + print(f"initial_obs: {obs_initial}") # action to be performed - # @todo why does a non zero action not change the observed value - non_zero_action = OrderedDict([('joints', np.array([1.2854844, 1.0638355, -1.9478809, -1.8864504, - -0.09758008, 4.125365, 0.19993608], dtype=np.float32))]) - - print(f"initial_obs: {obs_}") + non_zero_action = env_.action_space.sample() + # TODO, there seems to be a bug, it is required to step two times to see an effect obs, reward, terminated, truncated, info = env_.step(non_zero_action) print(f"{obs=}") - print(f"{obs['tquart'] == obs_['tquart']}") + assert not np.array_equal(obs['tquart'], obs_initial['tquart']) + @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") def test_collision_guard(self): """ Check that an obvious collision is detected by the CollisionGuard """ - # @todo how to test this functionality? cfg_ = sim.FR3Config() cfg_.ik_duration_in_milliseconds = 300 cfg_.realtime = False gripper_cfg = sim.FHConfig() - 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), - } + # no camera obs needed for this test + cameras = {} cam_cfg_ = SimCameraSetConfig(cameras=cameras, resolution_width=640, resolution_height=480, frame_rate=50) - env_ = tf.produce_env_sim_tquart_gripper_camera_cg_rel(mjcf_path=TestSimEnvs.mjcf_path, - urdf_path=TestSimEnvs.urdf_path, - cfg=cfg_, - gripper_cfg=gripper_cfg, - cam_cfg=cam_cfg_, - robot_id='0') + env_ = tf.produce_env_sim_joints_gripper_camera_cg(mjcf_path=TestSimEnvs.mjcf_path, + urdf_path=TestSimEnvs.urdf_path, + cfg=cfg_, + gripper_cfg=gripper_cfg, + cam_cfg=cam_cfg_, + robot_id='0') obs_, info_ = env_.reset() print(f"initial_obs: {obs_}") - for _ in range(2): - act = env_.action_space.sample() - # act["tquart"][3:] = [0, 0, 0, 1] - obs, reward, terminated, truncated, info = env_.step(act) - print(f"{obs=}") - if truncated or terminated: - env_.reset() + act = env_.action_space.sample() + # the below action is a test_case where there is an obvious collision regardless of the gripper action + act["joints"] = np.array([0, 1.78, 0, -1.45, 0, 0, 0], dtype=np.float32) + # TODO, there seems to be a bug, it is required to step two times to see an effect + obs, reward, terminated, truncated, info = env_.step(act) + assert info['collision'] From 9551099cc0fc83ca41fe4d4faeb0058451646bdf Mon Sep 17 00:00:00 2001 From: suman1209 Date: Fri, 2 Aug 2024 18:01:26 +0200 Subject: [PATCH 11/22] format(type_factory.py): formated code for isort issue and black format --- python/rcsss/envs/typed_factory.py | 368 ++++++++++++++++------------- 1 file changed, 207 insertions(+), 161 deletions(-) diff --git a/python/rcsss/envs/typed_factory.py b/python/rcsss/envs/typed_factory.py index f3c05698..6287c820 100644 --- a/python/rcsss/envs/typed_factory.py +++ b/python/rcsss/envs/typed_factory.py @@ -1,26 +1,32 @@ import typing + +import gymnasium as gym +from rcsss import hw, sim +from rcsss._core.hw import FHConfig from rcsss._core.sim import CameraType -from rcsss.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig from rcsss.camera.realsense import RealSenseCameraSet, RealSenseSetConfig -from rcsss.envs.hw import FR3HW -from rcsss._core.hw import FHConfig -from rcsss.envs.sim import CollisionGuard +from rcsss.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig +from rcsss.config import read_config_yaml +from rcsss.desk import Desk from rcsss.envs.base import ( + ArmObsType, + CameraSetWrapper, + CartOrJointContType, + ControlMode, + FR3Env, GripperWrapper, - RelativeActionSpace, - ObsArmsGrCam, + JointsDictType, + LimitedJointsRelDictType, LimitedTQuartRelDictType, LimitedTRPYRelDictType, - LimitedJointsRelDictType + ObsArmsGr, + ObsArmsGrCam, + RelativeActionSpace, + TQuartDictType, + TRPYDictType, ) -from rcsss import sim -from rcsss.envs.base import (ArmObsType, CameraSetWrapper, CartOrJointContType, ControlMode, FR3Env, - JointsDictType, TQuartDictType, TRPYDictType, ObsArmsGr) -from rcsss.envs.sim import FR3Sim -import gymnasium as gym -from rcsss import hw -from rcsss.config import read_config_yaml -from rcsss.desk import Desk +from rcsss.envs.hw import FR3HW +from rcsss.envs.sim import CollisionGuard, FR3Sim def produce_env_sim( @@ -31,42 +37,56 @@ def produce_env_sim( robot.set_parameters(cfg) env = FR3Env(robot, control_mode) env_sim = FR3Sim(env, simulation) - return typing.cast(gym.Env[ArmObsType, CartOrJointContType], FR3Sim(env_sim, simulation)), {"sim": simulation, - "robot": robot} + return typing.cast(gym.Env[ArmObsType, CartOrJointContType], FR3Sim(env_sim, simulation)), { + "sim": simulation, + "robot": robot, + } def produce_env_sim_joints( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id: str = "0" ) -> tuple[gym.Env[ArmObsType, JointsDictType], dict[str, typing.Any]]: env, info = produce_env_sim(mjcf_path, urdf_path, control_mode=ControlMode.JOINTS, cfg=cfg, robot_id=robot_id) - return typing.cast( - gym.Env[ArmObsType, JointsDictType], - env, - ), info + return ( + typing.cast( + gym.Env[ArmObsType, JointsDictType], + env, + ), + info, + ) def produce_env_sim_trpy( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id: str = "0" ) -> tuple[gym.Env[ArmObsType, TRPYDictType], dict[str, typing.Any]]: - env, info = produce_env_sim(mjcf_path, urdf_path, control_mode=ControlMode.CARTESIAN_TRPY, - cfg=cfg, robot_id=robot_id) - return typing.cast( - gym.Env[ArmObsType, TRPYDictType], - env, - ), info + env, info = produce_env_sim( + mjcf_path, urdf_path, control_mode=ControlMode.CARTESIAN_TRPY, cfg=cfg, robot_id=robot_id + ) + return ( + typing.cast( + gym.Env[ArmObsType, TRPYDictType], + env, + ), + info, + ) def produce_env_sim_tquart( mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, robot_id: str = "0" ) -> tuple[gym.Env[ArmObsType, TQuartDictType], dict[str, typing.Any]]: - env, info = produce_env_sim(mjcf_path, urdf_path, control_mode=ControlMode.CARTESIAN_TQuart, - cfg=cfg, robot_id=robot_id) + env, info = produce_env_sim( + mjcf_path, urdf_path, control_mode=ControlMode.CARTESIAN_TQuart, cfg=cfg, robot_id=robot_id + ) return typing.cast(gym.Env[ArmObsType, TQuartDictType], env), info def produce_env_sim_tquart_gripper_camera( - mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, - cam_cfg: SimCameraSetConfig, robot_id: str = "0" + mjcf_path: str, + urdf_path: str, + cfg: sim.FR3Config, + gripper_cfg: sim.FHConfig, + cam_cfg: SimCameraSetConfig, + robot_id: str = "0", ) -> gym.Env[ObsArmsGrCam, TQuartDictType]: """Environment with tquart, gripper, camera, collision guard and relative limits""" env_sim, info = produce_env_sim_tquart(mjcf_path, urdf_path, cfg, robot_id) @@ -82,8 +102,12 @@ def produce_env_sim_tquart_gripper_camera( def produce_env_sim_tquart_gripper_camera_rel( - mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, - cam_cfg: SimCameraSetConfig, robot_id: str = "0" + mjcf_path: str, + urdf_path: str, + cfg: sim.FR3Config, + gripper_cfg: sim.FHConfig, + cam_cfg: SimCameraSetConfig, + robot_id: str = "0", ) -> gym.Env[ObsArmsGrCam, LimitedTQuartRelDictType]: """Environment with tquart, gripper, camera, collision guard and relative limits""" env_cam = produce_env_sim_tquart_gripper_camera(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id) @@ -95,12 +119,17 @@ def produce_env_sim_tquart_gripper_camera_rel( def produce_env_sim_tquart_gripper_camera_cg( - mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, - cam_cfg: SimCameraSetConfig, robot_id: str = "0" + mjcf_path: str, + urdf_path: str, + cfg: sim.FR3Config, + gripper_cfg: sim.FHConfig, + cam_cfg: SimCameraSetConfig, + robot_id: str = "0", ) -> gym.Env[ObsArmsGrCam, TQuartDictType]: """Environment with tquart, gripper, camera, collision guard and relative limits""" - env_sim_tquart_gripper_cam = produce_env_sim_tquart_gripper_camera(mjcf_path, urdf_path, cfg, - gripper_cfg, cam_cfg, robot_id) + env_sim_tquart_gripper_cam = produce_env_sim_tquart_gripper_camera( + 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, @@ -115,12 +144,17 @@ def produce_env_sim_tquart_gripper_camera_cg( def produce_env_sim_tquart_gripper_camera_cg_rel( - mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, - cam_cfg: SimCameraSetConfig, robot_id: str = "0" + mjcf_path: str, + urdf_path: str, + cfg: sim.FR3Config, + gripper_cfg: sim.FHConfig, + cam_cfg: SimCameraSetConfig, + robot_id: str = "0", ) -> gym.Env[ObsArmsGrCam, LimitedTQuartRelDictType]: """Environment with tquart, gripper, camera, collision guard and relative limits""" - env_sim_tquart_gripper_cam = produce_env_sim_tquart_gripper_camera_cg(mjcf_path, urdf_path, cfg, - gripper_cfg, cam_cfg, robot_id) + env_sim_tquart_gripper_cam = produce_env_sim_tquart_gripper_camera_cg( + mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id + ) env_sim_tquart_gripper_cam_cg = RelativeActionSpace(env_sim_tquart_gripper_cam) return typing.cast( gym.Env[ObsArmsGrCam, LimitedTQuartRelDictType], @@ -129,8 +163,12 @@ def produce_env_sim_tquart_gripper_camera_cg_rel( def produce_env_sim_trpy_gripper_camera( - mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, - cam_cfg: SimCameraSetConfig, robot_id: str = "0" + mjcf_path: str, + urdf_path: str, + cfg: sim.FR3Config, + gripper_cfg: sim.FHConfig, + cam_cfg: SimCameraSetConfig, + robot_id: str = "0", ) -> gym.Env[ObsArmsGrCam, TRPYDictType]: """Environment with trpy, gripper, camera, collision guard and relative limits""" env_sim_trpy, info = produce_env_sim_trpy(mjcf_path, urdf_path, cfg, robot_id) @@ -146,12 +184,17 @@ def produce_env_sim_trpy_gripper_camera( def produce_env_sim_trpy_gripper_camera_rel( - mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, - cam_cfg: SimCameraSetConfig, robot_id: str = "0" + mjcf_path: str, + urdf_path: str, + cfg: sim.FR3Config, + gripper_cfg: sim.FHConfig, + cam_cfg: SimCameraSetConfig, + robot_id: str = "0", ) -> gym.Env[ObsArmsGrCam, TRPYDictType]: """Environment with trpy, gripper, camera, collision guard and relative limits""" - env_sim_trpy_gripper_cam = produce_env_sim_trpy_gripper_camera(mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, - robot_id) + env_sim_trpy_gripper_cam = produce_env_sim_trpy_gripper_camera( + mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id + ) env_sim_trpy_gripper_cam_rel = RelativeActionSpace(env_sim_trpy_gripper_cam) return typing.cast( gym.Env[ObsArmsGrCam, LimitedTRPYRelDictType], @@ -160,12 +203,17 @@ def produce_env_sim_trpy_gripper_camera_rel( def produce_env_sim_trpy_gripper_camera_cg( - mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, - cam_cfg: SimCameraSetConfig, robot_id: str = "0" + mjcf_path: str, + urdf_path: str, + cfg: sim.FR3Config, + gripper_cfg: sim.FHConfig, + cam_cfg: SimCameraSetConfig, + robot_id: str = "0", ) -> gym.Env[ObsArmsGrCam, TRPYDictType]: """Environment with trpy, gripper, camera, collision guard and relative limits""" - env_sim_trpy_gripper_cam = produce_env_sim_trpy_gripper_camera(mjcf_path, urdf_path, cfg, - gripper_cfg, cam_cfg, robot_id) + env_sim_trpy_gripper_cam = produce_env_sim_trpy_gripper_camera( + 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, @@ -180,12 +228,17 @@ def produce_env_sim_trpy_gripper_camera_cg( def produce_env_sim_trpy_gripper_camera_cg_rel( - mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, - cam_cfg: SimCameraSetConfig, robot_id: str = "0" + mjcf_path: str, + urdf_path: str, + cfg: sim.FR3Config, + gripper_cfg: sim.FHConfig, + cam_cfg: SimCameraSetConfig, + robot_id: str = "0", ) -> gym.Env[ObsArmsGrCam, LimitedTRPYRelDictType]: """Environment with trpy, gripper, camera, collision guard and relative limits""" - env_sim_trpy_gripper_camera_cg = produce_env_sim_trpy_gripper_camera_cg(mjcf_path, urdf_path, cfg, - gripper_cfg, cam_cfg, robot_id) + env_sim_trpy_gripper_camera_cg = produce_env_sim_trpy_gripper_camera_cg( + mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id + ) env_sim_trpy_gripper_camera_cg_rel = RelativeActionSpace(env_sim_trpy_gripper_camera_cg) return typing.cast( gym.Env[ObsArmsGrCam, LimitedTRPYRelDictType], @@ -194,8 +247,12 @@ def produce_env_sim_trpy_gripper_camera_cg_rel( def produce_env_sim_joints_gripper_camera( - mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, - cam_cfg: SimCameraSetConfig, robot_id: str = "0" + mjcf_path: str, + urdf_path: str, + cfg: sim.FR3Config, + gripper_cfg: sim.FHConfig, + cam_cfg: SimCameraSetConfig, + robot_id: str = "0", ) -> gym.Env[ObsArmsGrCam, JointsDictType]: """Environment with joints, gripper, camera""" env_sim_joints, info = produce_env_sim_joints(mjcf_path, urdf_path, cfg, robot_id) @@ -211,12 +268,17 @@ def produce_env_sim_joints_gripper_camera( def produce_env_sim_joints_gripper_camera_rel( - mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, - cam_cfg: SimCameraSetConfig, robot_id: str = "0" + mjcf_path: str, + urdf_path: str, + cfg: sim.FR3Config, + gripper_cfg: sim.FHConfig, + cam_cfg: SimCameraSetConfig, + robot_id: str = "0", ) -> gym.Env[ObsArmsGrCam, JointsDictType]: """Environment with joints, gripper, camera, collision guard and relative limits""" - env_sim_joints_cam = produce_env_sim_joints_gripper_camera(mjcf_path, urdf_path, cfg, - gripper_cfg, cam_cfg, robot_id) + env_sim_joints_cam = produce_env_sim_joints_gripper_camera( + mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id + ) env_sim_joints_cam_rel = RelativeActionSpace(env_sim_joints_cam) return typing.cast( gym.Env[ObsArmsGrCam, LimitedJointsRelDictType], @@ -225,12 +287,17 @@ def produce_env_sim_joints_gripper_camera_rel( def produce_env_sim_joints_gripper_camera_cg( - mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, - cam_cfg: SimCameraSetConfig, robot_id: str = "0" + mjcf_path: str, + urdf_path: str, + cfg: sim.FR3Config, + gripper_cfg: sim.FHConfig, + cam_cfg: SimCameraSetConfig, + robot_id: str = "0", ) -> gym.Env[ObsArmsGrCam, JointsDictType]: """Environment with joints, gripper, camera, collision guard and relative limits""" - env_sim_joints_gripper_cam = produce_env_sim_joints_gripper_camera(mjcf_path, urdf_path, cfg, - gripper_cfg, cam_cfg, robot_id) + env_sim_joints_gripper_cam = produce_env_sim_joints_gripper_camera( + 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, @@ -245,12 +312,17 @@ def produce_env_sim_joints_gripper_camera_cg( def produce_env_sim_joints_gripper_camera_cg_rel( - mjcf_path: str, urdf_path: str, cfg: sim.FR3Config, gripper_cfg: sim.FHConfig, - cam_cfg: SimCameraSetConfig, robot_id: str = "0" + mjcf_path: str, + urdf_path: str, + cfg: sim.FR3Config, + gripper_cfg: sim.FHConfig, + cam_cfg: SimCameraSetConfig, + robot_id: str = "0", ) -> gym.Env[ObsArmsGrCam, LimitedJointsRelDictType]: """Environment with joints, gripper, camera, collision guard and relative limits""" - env_sim_joints_gripper_cam_cg = produce_env_sim_joints_gripper_camera_cg(mjcf_path, urdf_path, cfg, gripper_cfg, - cam_cfg, robot_id) + env_sim_joints_gripper_cam_cg = produce_env_sim_joints_gripper_camera_cg( + mjcf_path, urdf_path, cfg, gripper_cfg, cam_cfg, robot_id + ) env_sim_joints_gripper_cam_cg_rel = RelativeActionSpace(env_sim_joints_gripper_cam_cg) return typing.cast( gym.Env[ObsArmsGrCam, LimitedJointsRelDictType], @@ -258,8 +330,9 @@ def produce_env_sim_joints_gripper_camera_cg_rel( ) -def produce_env_hw(ip: str, urdf_path: str, control_mode: ControlMode, cfg_path: str)\ - -> gym.Env[ArmObsType, CartOrJointContType]: +def produce_env_hw( + ip: str, urdf_path: str, control_mode: ControlMode, cfg_path: str +) -> gym.Env[ArmObsType, CartOrJointContType]: cfg = read_config_yaml(cfg_path) with Desk(ip, cfg.hw.username, cfg.hw.password) as d: d.unlock() @@ -271,61 +344,51 @@ def produce_env_hw(ip: str, urdf_path: str, control_mode: ControlMode, cfg_path: return typing.cast(gym.Env[ArmObsType, CartOrJointContType], env_hw) -def produce_env_hw_joints(ip: str, urdf_path: str, cfg_path: str)\ - -> gym.Env[ArmObsType, JointsDictType]: +def produce_env_hw_joints(ip: str, urdf_path: str, cfg_path: str) -> gym.Env[ArmObsType, JointsDictType]: env_hw_joints = produce_env_hw(ip, urdf_path, ControlMode.JOINTS, cfg_path) return typing.cast(gym.Env[ArmObsType, JointsDictType], env_hw_joints) -def produce_env_hw_joints_gripper(ip: str, urdf_path: str, - gripper_cfg: FHConfig, cfg_path: str) -> ( - gym.Env)[ObsArmsGr, JointsDictType]: +def produce_env_hw_joints_gripper( + ip: str, urdf_path: str, gripper_cfg: FHConfig, cfg_path: str +) -> (gym.Env)[ObsArmsGr, JointsDictType]: env_hw_joints = produce_env_hw_joints(ip, urdf_path, cfg_path) gripper = hw.FrankaHand(ip, gripper_cfg) env_hw_joints_gripper = GripperWrapper(env_hw_joints, gripper) return typing.cast(gym.Env[ObsArmsGr, JointsDictType], env_hw_joints_gripper) -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]: +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]: env_hw_joints_gripper = produce_env_hw_joints_gripper(ip, urdf_path, gripper_cfg, cfg_path) camera_set = RealSenseCameraSet(cam_cfg) env_hw_joints_gripper_cam: gym.Env = CameraSetWrapper(env_hw_joints_gripper, camera_set) return typing.cast(gym.Env[ObsArmsGrCam, JointsDictType], env_hw_joints_gripper_cam) -def produce_env_hw_joints_gripper_camera_rel(ip: str, urdf_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) +def produce_env_hw_joints_gripper_camera_rel( + ip: str, urdf_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_sim_joints_gripper_cam_rel = RelativeActionSpace(env_hw_joints_gripper_cam) return typing.cast(gym.Env[ObsArmsGrCam, LimitedJointsRelDictType], env_sim_joints_gripper_cam_rel) -def produce_env_hw_joints_gripper_camera_cg(ip: str, urdf_path: str, mjcf_path: str, - gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, - cfg_path: str)\ - -> gym.Env[ObsArmsGrCam, JointsDictType]: - env_hw_joints_gripper_cam = produce_env_hw_joints_gripper_camera(ip, urdf_path, - gripper_cfg, cam_cfg, cfg_path) +def produce_env_hw_joints_gripper_camera_cg( + ip: str, urdf_path: str, mjcf_path: str, gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, cfg_path: str +) -> 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 ) return typing.cast(gym.Env[ObsArmsGrCam, JointsDictType], env_hw_joints_gripper_cam_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) +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, @@ -336,59 +399,51 @@ def produce_env_hw_joints_gripper_camera_cg_rel(ip: str, urdf_path: str, mjcf_pa return typing.cast(gym.Env[ObsArmsGrCam, LimitedJointsRelDictType], env_hw_joints_gripper_cam_cg) -def produce_env_hw_trpy(ip: str, urdf_path: str, cfg_path: str)\ - -> gym.Env[ArmObsType, TRPYDictType]: +def produce_env_hw_trpy(ip: str, urdf_path: str, cfg_path: str) -> gym.Env[ArmObsType, TRPYDictType]: env_hw_trpy = produce_env_hw(ip, urdf_path, ControlMode.CARTESIAN_TRPY, cfg_path) return typing.cast(gym.Env[ArmObsType, TRPYDictType], env_hw_trpy) -def produce_env_hw_trpy_gripper(ip: str, urdf_path: str, - gripper_cfg: FHConfig, cfg_path: str) -> ( - gym.Env)[ObsArmsGr, TRPYDictType]: +def produce_env_hw_trpy_gripper( + ip: str, urdf_path: str, gripper_cfg: FHConfig, cfg_path: str +) -> (gym.Env)[ObsArmsGr, TRPYDictType]: env_hw_joints = produce_env_hw_trpy(ip, urdf_path, cfg_path) gripper = hw.FrankaHand(ip, gripper_cfg) env_hw_trpy_gripper = GripperWrapper(env_hw_joints, gripper) return typing.cast(gym.Env[ObsArmsGr, TRPYDictType], env_hw_trpy_gripper) -def produce_env_hw_trpy_gripper_camera(ip: str, urdf_path: str, gripper_cfg: FHConfig, - cam_cfg: RealSenseSetConfig, cfg_path: str) \ - -> gym.Env[ObsArmsGrCam, TRPYDictType]: +def produce_env_hw_trpy_gripper_camera( + ip: str, urdf_path: str, gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, cfg_path: str +) -> gym.Env[ObsArmsGrCam, TRPYDictType]: env_hw_trpy_gripper = produce_env_hw_trpy_gripper(ip, urdf_path, gripper_cfg, cfg_path) camera_set = RealSenseCameraSet(cam_cfg) env_hw_trpy_gripper_cam: gym.Env = CameraSetWrapper(env_hw_trpy_gripper, camera_set) return typing.cast(gym.Env[ObsArmsGrCam, TRPYDictType], env_hw_trpy_gripper_cam) -def produce_env_hw_trpy_gripper_camera_rel(ip: str, urdf_path: str, - gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, - cfg_path: str) \ - -> gym.Env[ObsArmsGrCam, LimitedTRPYRelDictType]: +def produce_env_hw_trpy_gripper_camera_rel( + ip: str, urdf_path: str, gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, cfg_path: str +) -> 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_trpy_gripper_cam_rel = RelativeActionSpace(env_hw_trpy_gripper_cam) return typing.cast(gym.Env[ObsArmsGrCam, LimitedTRPYRelDictType], env_hw_trpy_gripper_cam_rel) -def produce_env_hw_trpy_gripper_camera_cg(ip: str, urdf_path: str, mjcf_path: str, - gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, - cfg_path: str)\ - -> gym.Env[ObsArmsGrCam, TRPYDictType]: +def produce_env_hw_trpy_gripper_camera_cg( + ip: str, urdf_path: str, mjcf_path: str, gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, cfg_path: str +) -> 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 ) return typing.cast(gym.Env[ObsArmsGrCam, TRPYDictType], env_hw_trpy_gripper_cam_cg) -def produce_env_hw_trpy_gripper_camera_cg_rel(ip: str, urdf_path: str, mjcf_path: str, gripper_cfg: FHConfig, - cam_cfg: RealSenseSetConfig, cfg_path: str)\ - -> gym.Env[ObsArmsGrCam, LimitedTRPYRelDictType]: - env_hw_trpy_gripper_cam = produce_env_hw_trpy_gripper_camera(ip, urdf_path, gripper_cfg, - cam_cfg, cfg_path) +def produce_env_hw_trpy_gripper_camera_cg_rel( + ip: str, urdf_path: str, mjcf_path: str, gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, cfg_path: str +) -> 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, @@ -399,59 +454,53 @@ def produce_env_hw_trpy_gripper_camera_cg_rel(ip: str, urdf_path: str, mjcf_path return typing.cast(gym.Env[ObsArmsGrCam, LimitedTRPYRelDictType], env_hw_joints_gripper_cam_cg) -def produce_env_hw_tquart(ip: str, urdf_path: str, cfg_path: str)\ - -> gym.Env[ArmObsType, TQuartDictType]: +def produce_env_hw_tquart(ip: str, urdf_path: str, cfg_path: str) -> gym.Env[ArmObsType, TQuartDictType]: env_hw_tquart = produce_env_hw(ip, urdf_path, ControlMode.CARTESIAN_TQuart, cfg_path) return typing.cast(gym.Env[ArmObsType, TQuartDictType], env_hw_tquart) -def produce_env_hw_tquart_gripper(ip: str, urdf_path: str, gripper_cfg: FHConfig, cfg_path: str) -> ( - gym.Env)[ObsArmsGr, TQuartDictType]: +def produce_env_hw_tquart_gripper( + ip: str, urdf_path: str, gripper_cfg: FHConfig, cfg_path: str +) -> (gym.Env)[ObsArmsGr, TQuartDictType]: env_hw_tquart = produce_env_hw_tquart(ip, urdf_path, cfg_path) gripper = hw.FrankaHand(ip, gripper_cfg) env_hw_tquart_gripper = GripperWrapper(env_hw_tquart, gripper) return typing.cast(gym.Env[ObsArmsGr, TQuartDictType], env_hw_tquart_gripper) -def produce_env_hw_tquart_gripper_camera(ip: str, urdf_path: str, gripper_cfg: FHConfig, - cam_cfg: RealSenseSetConfig, cfg_path: str) \ - -> gym.Env[ObsArmsGrCam, TQuartDictType]: +def produce_env_hw_tquart_gripper_camera( + ip: str, urdf_path: str, gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, cfg_path: str +) -> gym.Env[ObsArmsGrCam, TQuartDictType]: env_hw_tquart_gripper = produce_env_hw_tquart_gripper(ip, urdf_path, gripper_cfg, cfg_path) camera_set = RealSenseCameraSet(cam_cfg) env_hw_tquart_gripper_cam: gym.Env = CameraSetWrapper(env_hw_tquart_gripper, camera_set) return typing.cast(gym.Env[ObsArmsGrCam, TQuartDictType], env_hw_tquart_gripper_cam) -def produce_env_hw_tquart_gripper_camera_rel(ip: str, urdf_path: str, - gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, - cfg_path: str) \ - -> gym.Env[ObsArmsGrCam, LimitedTQuartRelDictType]: - env_hw_tquart_gripper_cam = produce_env_hw_tquart_gripper_camera(ip, urdf_path, - gripper_cfg, cam_cfg, cfg_path) +def produce_env_hw_tquart_gripper_camera_rel( + ip: str, urdf_path: str, gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, cfg_path: str +) -> gym.Env[ObsArmsGrCam, LimitedTQuartRelDictType]: + env_hw_tquart_gripper_cam = produce_env_hw_tquart_gripper_camera(ip, urdf_path, gripper_cfg, cam_cfg, cfg_path) env_sim_tquart_gripper_cam_rel = RelativeActionSpace(env_hw_tquart_gripper_cam) return typing.cast(gym.Env[ObsArmsGrCam, LimitedTQuartRelDictType], env_sim_tquart_gripper_cam_rel) -def produce_env_hw_tquart_gripper_camera_cg(ip: str, urdf_path: str, mjcf_path: str, - gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, cfg_path: str)\ - -> gym.Env[ObsArmsGrCam, TQuartDictType]: - env_hw_tquart_gripper_cam = produce_env_hw_tquart_gripper_camera(ip, urdf_path, - gripper_cfg, cam_cfg, cfg_path) +def produce_env_hw_tquart_gripper_camera_cg( + ip: str, urdf_path: str, mjcf_path: str, gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, cfg_path: str +) -> 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 ) return typing.cast(gym.Env[ObsArmsGrCam, TQuartDictType], env_hw_tquart_gripper_cam_cg) -def produce_env_hw_tquart_gripper_camera_cg_rel(ip: str, urdf_path: str, mjcf_path: str, gripper_cfg: FHConfig, - cam_cfg: RealSenseSetConfig, cfg_path: str)\ - -> gym.Env[ObsArmsGrCam, LimitedTQuartRelDictType]: - env_hw_tquart_gripper_cam = produce_env_hw_tquart_gripper_camera_cg(ip, urdf_path, mjcf_path, gripper_cfg, - cam_cfg, cfg_path) +def produce_env_hw_tquart_gripper_camera_cg_rel( + ip: str, urdf_path: str, mjcf_path: str, gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, cfg_path: str +) -> gym.Env[ObsArmsGrCam, LimitedTQuartRelDictType]: + env_hw_tquart_gripper_cam = produce_env_hw_tquart_gripper_camera_cg( + 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, @@ -478,12 +527,9 @@ def produce_env_hw_tquart_gripper_camera_cg_rel(ip: str, urdf_path: str, mjcf_pa } cam_cfg_ = SimCameraSetConfig(cameras=cameras, resolution_width=640, resolution_height=480, frame_rate=50) - env_ = produce_env_sim_tquart_gripper_camera_rel(mjcf_path=mjcf, - urdf_path=urdf, - cfg=cfg_, - gripper_cfg=gripper_cfg, - cam_cfg=cam_cfg_, - robot_id=robot_id_) + env_ = produce_env_sim_tquart_gripper_camera_rel( + mjcf_path=mjcf, urdf_path=urdf, cfg=cfg_, gripper_cfg=gripper_cfg, cam_cfg=cam_cfg_, robot_id=robot_id_ + ) obs_, info_ = env_.reset() for _ in range(100): act = env_.action_space.sample() From 4139d11286fbccf2696d8e67687b8351e0e3eae9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 15 Aug 2024 15:45:41 +0200 Subject: [PATCH 12/22] style(tests): format typed factory tests --- python/tests/test_sim_envs.py | 34 +++++++++++++++++++++------------- 1 file changed, 21 insertions(+), 13 deletions(-) diff --git a/python/tests/test_sim_envs.py b/python/tests/test_sim_envs.py index 6ef01160..4534be78 100644 --- a/python/tests/test_sim_envs.py +++ b/python/tests/test_sim_envs.py @@ -1,6 +1,7 @@ import pytest from collections import OrderedDict import numpy as np +import rcsss from rcsss._core.sim import CameraType from rcsss.camera.sim import SimCameraConfig, SimCameraSetConfig from rcsss.envs import typed_factory as tf @@ -8,8 +9,13 @@ class TestSimEnvs: - mjcf_path = "models/mjcf/fr3_modular/scene.xml" - urdf_path = "models/fr3/urdf/fr3_from_panda.urdf" + mjcf_path = str(rcsss.scenes["fr3_empty_world"]) + urdf_path = str(rcsss.scenes["lab"].parent / "fr3.urdf") + + def test_model_availability(self): + assert ( + "lab" in rcsss.scenes + ), "This pip package was not built with the UTN lab models which is needed for these tests, aborting." @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") def test_zero_action_trpy(self): @@ -23,12 +29,12 @@ def test_zero_action_trpy(self): obs_initial, info_ = env.reset() print(f"initial_obs: {obs_initial}") print(f"info: {info_}") - zero_action = OrderedDict([('xyzrpy', np.array([0, 0, 0, 0, 0, 0], dtype=np.float32))]) + zero_action = OrderedDict([("xyzrpy", np.array([0, 0, 0, 0, 0, 0], dtype=np.float32))]) print(f"zero_action: {zero_action}") # TODO, there seems to be a bug, with two steps of this zero action, the observation changes obs, reward, terminated, truncated, info = env.step(zero_action) print(f"{obs=}") - assert np.array_equal(obs['tquart'], obs_initial['tquart']) + assert np.array_equal(obs["tquart"], obs_initial["tquart"]) @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") def test_non_zero_action_trpy(self): @@ -49,7 +55,7 @@ def test_non_zero_action_trpy(self): # TODO, there seems to be a bug, it is required to step two times to see an effect obs, reward, terminated, truncated, info = env.step(non_zero_action) print(f"{obs=}") - assert not np.array_equal(obs['tquart'], obs_initial['tquart']) + assert not np.array_equal(obs["tquart"], obs_initial["tquart"]) @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") def test_non_zero_action_joints(self): @@ -67,7 +73,7 @@ def test_non_zero_action_joints(self): # TODO, there seems to be a bug, it is required to step two times to see an effect obs, reward, terminated, truncated, info = env_.step(non_zero_action) print(f"{obs=}") - assert not np.array_equal(obs['tquart'], obs_initial['tquart']) + assert not np.array_equal(obs["tquart"], obs_initial["tquart"]) @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") def test_collision_guard(self): @@ -83,12 +89,14 @@ def test_collision_guard(self): cameras = {} cam_cfg_ = SimCameraSetConfig(cameras=cameras, resolution_width=640, resolution_height=480, frame_rate=50) - env_ = tf.produce_env_sim_joints_gripper_camera_cg(mjcf_path=TestSimEnvs.mjcf_path, - urdf_path=TestSimEnvs.urdf_path, - cfg=cfg_, - gripper_cfg=gripper_cfg, - cam_cfg=cam_cfg_, - robot_id='0') + env_ = tf.produce_env_sim_joints_gripper_camera_cg( + mjcf_path=TestSimEnvs.mjcf_path, + urdf_path=TestSimEnvs.urdf_path, + cfg=cfg_, + gripper_cfg=gripper_cfg, + cam_cfg=cam_cfg_, + robot_id="0", + ) obs_, info_ = env_.reset() print(f"initial_obs: {obs_}") act = env_.action_space.sample() @@ -96,4 +104,4 @@ def test_collision_guard(self): act["joints"] = np.array([0, 1.78, 0, -1.45, 0, 0, 0], dtype=np.float32) # TODO, there seems to be a bug, it is required to step two times to see an effect obs, reward, terminated, truncated, info = env_.step(act) - assert info['collision'] + assert info["collision"] From 848330741da29f368fa2bf73a130679c30344b10 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 15 Aug 2024 16:17:33 +0200 Subject: [PATCH 13/22] fix(envs): fixed sim env returned obs --- python/rcsss/envs/base.py | 3 ++- python/rcsss/envs/hw.py | 4 ++-- python/rcsss/envs/sim.py | 5 +++-- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/python/rcsss/envs/base.py b/python/rcsss/envs/base.py index cb2255e5..9f42bf1b 100644 --- a/python/rcsss/envs/base.py +++ b/python/rcsss/envs/base.py @@ -147,7 +147,8 @@ class FR3Env(gym.Env): Top view of on the robot. Robot faces into x direction. z direction faces upwards. (Right handed coordinate axis) ^ x - <- RobotBase + | + <-- RobotBase y """ diff --git a/python/rcsss/envs/hw.py b/python/rcsss/envs/hw.py index 8724ebe6..db396c5e 100644 --- a/python/rcsss/envs/hw.py +++ b/python/rcsss/envs/hw.py @@ -19,7 +19,7 @@ def __init__(self, env): assert isinstance(self.unwrapped.robot, hw.FR3), "Robot must be a hw.FR3 instance." self.hw_robot = cast(hw.FR3, self.unwrapped.robot) - def step(self, action: Any) -> tuple[Any, SupportsFloat, bool, bool, dict]: + def step(self, action: Any) -> tuple[dict[str, Any], SupportsFloat, bool, bool, dict]: try: return super().step(action) except hw.exceptions.FrankaControlException as e: @@ -27,7 +27,7 @@ def step(self, action: Any) -> tuple[Any, SupportsFloat, bool, bool, dict]: self.hw_robot.automatic_error_recovery() # TODO: this does not work if some wrappers are in between # FR3HW and FR3Env - return self.unwrapped.get_obs(), 0, False, True, {} + return dict(self.unwrapped.get_obs()), 0, False, True, {} def reset( self, seed: int | None = None, options: dict[str, Any] | None = None diff --git a/python/rcsss/envs/sim.py b/python/rcsss/envs/sim.py index 8390644b..24b51d23 100644 --- a/python/rcsss/envs/sim.py +++ b/python/rcsss/envs/sim.py @@ -25,14 +25,15 @@ def __init__(self, env, simulation: sim.Sim): self.sim = simulation def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, bool, dict]: - obs, _, _, _, info = super().step(action) + _, _, _, _, info = super().step(action) self.sim.step_until_convergence() state = self.sim_robot.get_state() info["collision"] = state.collision info["ik_success"] = state.ik_success info["is_sim_converged"] = self.sim.is_converged() # truncate episode if collision - return obs, 0, False, state.collision or not state.ik_success, info + + return dict(self.unwrapped.get_obs()), 0, False, state.collision or not state.ik_success, info def reset( self, seed: int | None = None, options: dict[str, Any] | None = None From c74f695a56e094a276146775afa0d761dbc4dcf9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 15 Aug 2024 16:17:40 +0200 Subject: [PATCH 14/22] ci: added models download to allow model dependent tests --- .github/workflows/py.yaml | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/.github/workflows/py.yaml b/.github/workflows/py.yaml index 0e83a85c..66dc6a8d 100644 --- a/.github/workflows/py.yaml +++ b/.github/workflows/py.yaml @@ -41,6 +41,7 @@ jobs: env: CC: clang-15 CXX: clang++-15 + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} steps: - uses: actions/checkout@v4 - name: Install CPP dependencies @@ -52,10 +53,12 @@ jobs: cache: "pip" - name: Install building dependencies run: python -m pip install -r requirements_dev.txt - - name: Create python wheel - run: python -m build --wheel --outdir dist/ - - name: Install the wheel package - run: python -m pip install dist/*.whl + - name: Download the models using cmake + env: + MODEL_TOKEN: ${{ secrets.MODEL_TOKEN }} + run: cmake -G Ninja -B build -DINCLUDE_UTN_MODELS=ON -DGITLAB_MODELS_TOKEN="$MODEL_TOKEN" -DMUJOCO_VERSION=3.1.5 + - name: Build the python package + run: pip install -v . - name: Check that stub files are up-to-date run: make stubgen && git diff --exit-code - name: Ensure all unittests(pytest) are passing From 477f06cd700b166e349441be787dd249bfbc6809 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 22 Aug 2024 12:01:06 +0200 Subject: [PATCH 15/22] docs(readme): fixed install requirements --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index b695e27f..e2af3ea7 100644 --- a/README.md +++ b/README.md @@ -12,7 +12,7 @@ sudo apt install $(cat debian_deps.txt) python3 -m venv .venv source .venv/bin/activate pip config --site set global.no-build-isolation false -pip install --requirements requirements_dev.txt +pip install -r requirements_dev.txt ``` 3. Build and install RCS: ```shell From f9b057ac532fd7c82897e1c36d245bb08f93a4dc Mon Sep 17 00:00:00 2001 From: suman1209 Date: Thu, 22 Aug 2024 12:28:37 +0200 Subject: [PATCH 16/22] test(test_sim_envs): added more test cases --- python/rcsss/envs/base.py | 4 - python/tests/test_sim_envs.py | 323 +++++++++++++++++++++++++++++----- 2 files changed, 282 insertions(+), 45 deletions(-) diff --git a/python/rcsss/envs/base.py b/python/rcsss/envs/base.py index 7e372dec..9b0e1be8 100644 --- a/python/rcsss/envs/base.py +++ b/python/rcsss/envs/base.py @@ -119,10 +119,6 @@ class ArmObsType(TQuartDictType, JointsDictType): ... LimitedCartOrJointContType: TypeAlias = LimitedTQuartRelDictType | LimitedJointsRelDictType | LimitedTRPYRelDictType -class ActionsRelative(LimitedJointsRelDictType): - pass - - class ObsArmsGr(ArmObsType, GripperDictType): pass diff --git a/python/tests/test_sim_envs.py b/python/tests/test_sim_envs.py index 4534be78..4dc30cb5 100644 --- a/python/tests/test_sim_envs.py +++ b/python/tests/test_sim_envs.py @@ -1,14 +1,38 @@ import pytest from collections import OrderedDict import numpy as np +from rcsss._core import common + import rcsss -from rcsss._core.sim import CameraType -from rcsss.camera.sim import SimCameraConfig, SimCameraSetConfig +from rcsss.camera.sim import SimCameraSetConfig from rcsss.envs import typed_factory as tf from rcsss import sim +@pytest.fixture +def cfg(): + fr3_config = sim.FR3Config() + fr3_config.ik_duration_in_milliseconds = 300 + fr3_config.realtime = False + return fr3_config + + +@pytest.fixture +def gripper_cfg(): + gripper_cfg = sim.FHConfig() + return gripper_cfg + + +@pytest.fixture +def cam_cfg(): + # no camera obs needed for this test + cameras = {} + cam_cfg_ = SimCameraSetConfig(cameras=cameras, resolution_width=640, resolution_height=480, frame_rate=50) + return cam_cfg_ + + class TestSimEnvs: + """This class is for testing common sim env functionalities""" mjcf_path = str(rcsss.scenes["fr3_empty_world"]) urdf_path = str(rcsss.scenes["lab"].parent / "fr3.urdf") @@ -17,91 +41,308 @@ def test_model_availability(self): "lab" in rcsss.scenes ), "This pip package was not built with the UTN lab models which is needed for these tests, aborting." + +class TestSimEnvsTRPY: + """This class is for testing TRPY sim env functionalities""" + @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") - def test_zero_action_trpy(self): + def test_zero_action_trpy(self, cfg): """ Test that a zero action does not change the state significantly """ - cfg_ = sim.FR3Config() - cfg_.ik_duration_in_milliseconds = 300 - cfg_.realtime = False - env, _ = tf.produce_env_sim_trpy(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg_, robot_id="0") + env, _ = tf.produce_env_sim_trpy(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg, robot_id="0") obs_initial, info_ = env.reset() print(f"initial_obs: {obs_initial}") print(f"info: {info_}") - zero_action = OrderedDict([("xyzrpy", np.array([0, 0, 0, 0, 0, 0], dtype=np.float32))]) + t = obs_initial["tquart"][:3] + q = obs_initial["tquart"][3:] + initial_pose = common.Pose(translation=np.array(t), quaternion=np.array(q)) + xyzrpy = t.tolist() + initial_pose.rotation_rpy().as_vector().tolist() + zero_action = OrderedDict([("xyzrpy", xyzrpy)]) print(f"zero_action: {zero_action}") - # TODO, there seems to be a bug, with two steps of this zero action, the observation changes obs, reward, terminated, truncated, info = env.step(zero_action) print(f"{obs=}") - assert np.array_equal(obs["tquart"], obs_initial["tquart"]) + """ + @todo Assumptions taken, + after env.reset() is called, the robot tcp will be in the home position, and then applying an action + (set_cartesian_position(home_position)) should not change the observation significantly. + + Is the precision of 1 mm reasonable? + """ + assert np.allclose(obs["tquart"], obs_initial["tquart"], atol=0.001, rtol=0) @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") - def test_non_zero_action_trpy(self): + def test_non_zero_action_trpy(self, cfg): """ This is for testing that a certain action leads to the expected change in state """ # env creation - cfg_ = sim.FR3Config() - cfg_.ik_duration_in_milliseconds = 300 - cfg_.realtime = False - env, _ = tf.produce_env_sim_trpy(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg_, robot_id="0") + env, _ = tf.produce_env_sim_trpy(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg, robot_id="0") + obs_initial, info_ = env.reset() + print(f"initial_obs: {obs_initial}") + # action to be performed` + x_pos_change = 0.2 + initial_tquart = obs_initial["tquart"].copy() + t = initial_tquart[:3] + # let's shift the translation in x + t[0] += x_pos_change + print(f"{t = }") + q = initial_tquart[3:] + initial_pose = common.Pose(translation=np.array(t), quaternion=np.array(q)) + xyzrpy = t.tolist() + initial_pose.rotation_rpy().as_vector().tolist() + non_zero_action = OrderedDict([("xyzrpy", np.array(xyzrpy)), + ('gripper', np.array([0], dtype=np.float32))]) + expected_obs = obs_initial["tquart"].copy() + expected_obs[0] += x_pos_change + print(f"expected_obs: {expected_obs}") + print(f"non_zero_action: {non_zero_action}") + obs, reward, terminated, truncated, info = env.step(non_zero_action) + print(f"{obs=}") + """ + @todo the test does not match the expected outputs + """ + assert np.allclose(obs["tquart"], expected_obs) + + @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") + def test_relative_zero_action(self, cfg, gripper_cfg, cam_cfg): + # env creation + env = tf.produce_env_sim_trpy_gripper_camera_cg_rel(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, + cfg, gripper_cfg, cam_cfg, robot_id="0") obs_initial, info_ = env.reset() print(f"initial_obs: {obs_initial}") # action to be performed - non_zero_action = env.action_space.sample() + zero_action = OrderedDict([("xyzrpy", np.array([0, 0, 0, 0, 0, 0], dtype=np.float32)), + ('gripper', np.array([0], dtype=np.float32))]) + print(f"zero_action: {zero_action}") + obs, reward, terminated, truncated, info = env.step(zero_action) + print(f"{obs=}") + """ + @todo + In this test case, a relative action of zeros should not lead to any change in the tcp trpy, but its not + functioning as expected.""" + assert np.allclose(obs["tquart"], obs_initial["tquart"], atol=0.001, rtol=0) + + @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") + def test_relative_non_zero_action(self, cfg, gripper_cfg, cam_cfg): + # env creation + env = tf.produce_env_sim_trpy_gripper_camera_cg_rel(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, + cfg, gripper_cfg, cam_cfg, robot_id="0") + obs_initial, info_ = env.reset() + + print(f"initial_obs: {obs_initial}") + # action to be performed + x_pos_change = 0.2 + non_zero_action = OrderedDict([("xyzrpy", np.array([x_pos_change, 0, 0, 0, 0, 0], dtype=np.float32)), + ('gripper', np.array([0], dtype=np.float32))]) + expected_obs = obs_initial["tquart"].copy() + + expected_obs[0] += x_pos_change + print(f"expected_obs: {expected_obs}") print(f"non_zero_action: {non_zero_action}") - # TODO, there seems to be a bug, it is required to step two times to see an effect obs, reward, terminated, truncated, info = env.step(non_zero_action) print(f"{obs=}") - assert not np.array_equal(obs["tquart"], obs_initial["tquart"]) + """@todo here I expect the final pose to be at least close within 1mm, is this reasonable?""" + assert np.allclose(obs["tquart"], expected_obs, atol=0.001, rtol=0) @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") - def test_non_zero_action_joints(self): + def test_collision_guard_trpy(self, cfg, gripper_cfg, cam_cfg): + """ + Check that an obvious collision is detected by the CollisionGuard + """ + + env_ = tf.produce_env_sim_trpy_gripper_camera_cg( + mjcf_path=TestSimEnvs.mjcf_path, + urdf_path=TestSimEnvs.urdf_path, + cfg=cfg, + gripper_cfg=gripper_cfg, + cam_cfg=cam_cfg, + robot_id="0", + ) + obs_, info_ = env_.reset() + print(f"initial_obs: {obs_}") + # an obvious below ground collision action + collision_action = OrderedDict([('xyzrpy', np.array([0.3, 0, -0.2, -0.13712998, -2.2763247, + -0.23976775], dtype=np.float32)), + ('gripper', np.array([0.18612409], dtype=np.float32))]) + obs, reward, terminated, truncated, info = env_.step(collision_action) + print(f'{obs = }') + print(f"info: {info}") + # """This is a scenario of the tcp below the ground which is an obvious collision, however the info.collision + # property is not populated correctly""" + assert info["collision"] + + +class TestSimEnvsTquart: + + """This class is for testing Tquart sim env functionalities""" + + def test_non_zero_action_tquart(self, cfg): + """ + Test that a zero action does not change the state significantly in the tquart configuration + """ + env, _ = tf.produce_env_sim_tquart(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg, robot_id="0") + obs_initial, info_ = env.reset() + print(f"initial_obs: {obs_initial}") + # action to be performed + t = obs_initial["tquart"][:3].tolist() + q = obs_initial["tquart"][3:].tolist() + x_pos_change = 0.3 + # updating the x action by 20cm + t[0] += x_pos_change + non_zero_action = OrderedDict([('tquart', np.array(t + q, dtype=np.float32))]) + + expected_obs = obs_initial["tquart"].copy() + expected_obs[0] += x_pos_change + print(f"non_zero_action: {non_zero_action}") + obs, reward, terminated, truncated, info = env.step(non_zero_action) + print(f"{obs=}") + assert np.allclose(obs["tquart"], expected_obs, atol=0.001, rtol=0) + + def test_zero_action_tquart(self, cfg): + """ + Test that a zero action does not change the state significantly in the tquart configuration + """ + env, _ = tf.produce_env_sim_tquart(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg, robot_id="0") + obs_initial, info_ = env.reset() + print(f"initial_obs: {obs_initial}") + print(f"info: {info_}") + home_action_vec = obs_initial["tquart"] + zero_action = OrderedDict([("tquart", home_action_vec)]) + print(f"zero_action: {zero_action}") + obs, reward, terminated, truncated, info = env.step(zero_action) + print(f"{obs=}") + """ + @todo Assumptions taken, + after env.reset() is called, the robot tcp will be in the home position, and then applying an action + (set_cartesian_position(home_position)) should not change the observation. + + But the observation doesnt match ...??? + + Is it reasonable to use atol=1mm and rtol=0 which means the two values can only vary less than 0.1 to be + considered close to each other. + """ + assert np.allclose(obs["tquart"], obs_initial["tquart"], atol=0.001, rtol=0) + + @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") + def test_relative_zero_action_tquart(self, cfg, gripper_cfg, cam_cfg): + # env creation + env = tf.produce_env_sim_tquart_gripper_camera_rel(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, + cfg, gripper_cfg, cam_cfg, robot_id="0") + obs_initial, info_ = env.reset() + print(f'{obs_initial = }') + zero_rel_action = OrderedDict([('tquart', np.array([0, 0, 0, 0, 0, 0, 0.], dtype=np.float32)), + ('gripper', np.array([0], dtype=np.float32))]) + print(f"zero_rel_action: {zero_rel_action}") + obs, reward, terminated, truncated, info = env.step(zero_rel_action) + print(f"{obs=}") + # """ + # @todo + # In this test case, a relative action of zeros should not lead to any change in the tcp trpy, but its not + # functioning as expected.""" + assert np.allclose(obs["tquart"], obs_initial["tquart"], atol=0.001, rtol=0) + + @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") + def test_collision_guard_tquart(self, cfg, gripper_cfg, cam_cfg): + """ + Check that an obvious collision is detected by the CollisionGuard + """ + + env_ = tf.produce_env_sim_tquart_gripper_camera_cg( + mjcf_path=TestSimEnvs.mjcf_path, + urdf_path=TestSimEnvs.urdf_path, + cfg=cfg, + gripper_cfg=gripper_cfg, + cam_cfg=cam_cfg, + robot_id="0", + ) + obs_, info_ = env_.reset() + print(f"initial_obs: {obs_}") + # this is a pose with an obvious collision (below the floor) + act = OrderedDict([('tquart', np.array([0.3, 0, -0.5, 9.23879533e-01, + -3.82683432e-01, -7.25288143e-17, -3.00424186e-17])), + ('gripper', np.array([0], dtype=np.float32))]) + print(f"act: {act}") + obs, reward, terminated, truncated, info = env_.step(act) + print(f'{obs = }') + print(f"info: {info}") + """ + @todo + This is a scenario of the tcp below the ground which is an obvious collision, however the info.collision + property is not populated collision_action""" + assert info["collision"] + + +class TestSimEnvsJoints: + """This class is for testing Joints sim env functionalities""" + def test_zero_action_joints(self, cfg): """ This is for testing that a certain action leads to the expected change in state """ - cfg_ = sim.FR3Config() - cfg_.ik_duration_in_milliseconds = 300 - cfg_.realtime = False - env_, _ = tf.produce_env_sim_joints(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg_, robot_id="0") + env_, _ = tf.produce_env_sim_joints(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg, robot_id="0") obs_initial, info_ = env_.reset() print(f"initial_obs: {obs_initial}") # action to be performed - non_zero_action = env_.action_space.sample() - # TODO, there seems to be a bug, it is required to step two times to see an effect - obs, reward, terminated, truncated, info = env_.step(non_zero_action) + zero_action = OrderedDict([('joints', np.array(obs_initial['joints']))]) + print(f"zero_action: {zero_action}") + obs, reward, terminated, truncated, info = env_.step(zero_action) print(f"{obs=}") - assert not np.array_equal(obs["tquart"], obs_initial["tquart"]) + assert np.allclose(obs["tquart"], obs_initial["tquart"], atol=0.001, rtol=0) @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") - def test_collision_guard(self): + def test_non_zero_action_joints(self, cfg): """ - Check that an obvious collision is detected by the CollisionGuard + This is for testing that a certain action leads to the expected change in state """ - cfg_ = sim.FR3Config() - cfg_.ik_duration_in_milliseconds = 300 - cfg_.realtime = False - gripper_cfg = sim.FHConfig() + env_, _ = tf.produce_env_sim_joints(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg, robot_id="0") + obs_initial, info_ = env_.reset() + print(f"initial_obs: {obs_initial}") + # action to be performed + non_zero_action = OrderedDict([('joints', np.array([0, 0.731, 0, 0, 0, 0, 0]))]) + print(f"non_zero_action: {non_zero_action}") + obs, reward, terminated, truncated, info = env_.step(non_zero_action) + print(f"{obs=}") + """@todo, If i set a particular joint angle, I assume that the observed joint angle must be the same""" + assert np.allclose(obs["joints"], non_zero_action["joints"], atol=0.001, rtol=0) - # no camera obs needed for this test - cameras = {} - cam_cfg_ = SimCameraSetConfig(cameras=cameras, resolution_width=640, resolution_height=480, frame_rate=50) + def test_collision_guard_joints(self, cfg, gripper_cfg, cam_cfg): + """ + Check that an obvious collision is detected by the CollisionGuard + """ env_ = tf.produce_env_sim_joints_gripper_camera_cg( mjcf_path=TestSimEnvs.mjcf_path, urdf_path=TestSimEnvs.urdf_path, - cfg=cfg_, + cfg=cfg, gripper_cfg=gripper_cfg, - cam_cfg=cam_cfg_, + cam_cfg=cam_cfg, robot_id="0", ) obs_, info_ = env_.reset() print(f"initial_obs: {obs_}") - act = env_.action_space.sample() # the below action is a test_case where there is an obvious collision regardless of the gripper action - act["joints"] = np.array([0, 1.78, 0, -1.45, 0, 0, 0], dtype=np.float32) - # TODO, there seems to be a bug, it is required to step two times to see an effect + act = OrderedDict([('joints', np.array([0, 1.78, 0, -1.45, 0, 0, 0], dtype=np.float32)), + ('gripper', np.array([0.5209108], dtype=np.float32))]) obs, reward, terminated, truncated, info = env_.step(act) assert info["collision"] + + def test_relative_zero_action_joints(self, cfg, gripper_cfg, cam_cfg): + """ + Check that an obvious collision is detected by the CollisionGuard + """ + + env_ = tf.produce_env_sim_joints_gripper_camera_rel( + mjcf_path=TestSimEnvs.mjcf_path, + urdf_path=TestSimEnvs.urdf_path, + cfg=cfg, + gripper_cfg=gripper_cfg, + cam_cfg=cam_cfg, + robot_id="0", + ) + obs_initial, info_ = env_.reset() + print(f"initial_obs: {obs_initial}") + act = OrderedDict([('joints', np.array([0, 0, 0, 0, 0, 0, 0], dtype=np.float32)), + ('gripper', np.array([0], dtype=np.float32))]) + print(f'{act = }') + obs, reward, terminated, truncated, info = env_.step(act) + assert np.allclose(obs["tquart"], obs_initial["tquart"], atol=0.001, rtol=0) From 5768aa0d035f89e615396620b2ab89b050103a56 Mon Sep 17 00:00:00 2001 From: suman1209 Date: Thu, 22 Aug 2024 16:06:48 +0200 Subject: [PATCH 17/22] test(test_sim_envs): upated assertion with pose.isclose() --- python/tests/test_sim_envs.py | 67 +++++++++++++++++++++++------------ 1 file changed, 45 insertions(+), 22 deletions(-) diff --git a/python/tests/test_sim_envs.py b/python/tests/test_sim_envs.py index 4dc30cb5..94ae2a7c 100644 --- a/python/tests/test_sim_envs.py +++ b/python/tests/test_sim_envs.py @@ -45,7 +45,7 @@ def test_model_availability(self): class TestSimEnvsTRPY: """This class is for testing TRPY sim env functionalities""" - @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") + # @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") def test_zero_action_trpy(self, cfg): """ Test that a zero action does not change the state significantly @@ -69,9 +69,11 @@ def test_zero_action_trpy(self, cfg): Is the precision of 1 mm reasonable? """ - assert np.allclose(obs["tquart"], obs_initial["tquart"], atol=0.001, rtol=0) + out_pose = common.Pose(translation=np.array(obs["tquart"][:3]), quaternion=np.array(obs["tquart"][3:])) + expected_pose = initial_pose + assert out_pose.is_close(expected_pose) - @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") + # @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") def test_non_zero_action_trpy(self, cfg): """ This is for testing that a certain action leads to the expected change in state @@ -101,29 +103,36 @@ def test_non_zero_action_trpy(self, cfg): """ @todo the test does not match the expected outputs """ - assert np.allclose(obs["tquart"], expected_obs) + out_pose = common.Pose(translation=np.array(obs["tquart"][:3]), quaternion=np.array(obs["tquart"][3:])) + expected_pose = common.Pose(translation=np.array(expected_obs[:3]), + quaternion=np.array(expected_obs[3:])) + assert out_pose.is_close(expected_pose) - @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") - def test_relative_zero_action(self, cfg, gripper_cfg, cam_cfg): + def test_relative_zero_action_trpy(self, cfg, gripper_cfg, cam_cfg): # env creation - env = tf.produce_env_sim_trpy_gripper_camera_cg_rel(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, + env = tf.produce_env_sim_trpy_gripper_camera_rel(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg, gripper_cfg, cam_cfg, robot_id="0") obs_initial, info_ = env.reset() print(f"initial_obs: {obs_initial}") # action to be performed - zero_action = OrderedDict([("xyzrpy", np.array([0, 0, 0, 0, 0, 0], dtype=np.float32)), + zero_action = OrderedDict([("xyzrpy", np.array([0, 0, 0, 0, 0, 0], dtype=np.float32)), ('gripper', np.array([0], dtype=np.float32))]) print(f"zero_action: {zero_action}") obs, reward, terminated, truncated, info = env.step(zero_action) print(f"{obs=}") + print(f"{info=}") """ @todo In this test case, a relative action of zeros should not lead to any change in the tcp trpy, but its not functioning as expected.""" - assert np.allclose(obs["tquart"], obs_initial["tquart"], atol=0.001, rtol=0) + # assert np.allclose(obs["tquart"], obs_initial["tquart"], atol=0.1, rtol=0) + out_pose = common.Pose(translation=np.array(obs["tquart"][:3]), quaternion=np.array(obs["tquart"][3:])) + expected_pose = common.Pose(translation=np.array(obs_initial["tquart"][:3]), + quaternion=np.array(obs_initial["tquart"][3:])) + assert out_pose.is_close(expected_pose) - @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") + # @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") def test_relative_non_zero_action(self, cfg, gripper_cfg, cam_cfg): # env creation env = tf.produce_env_sim_trpy_gripper_camera_cg_rel(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, @@ -143,9 +152,12 @@ def test_relative_non_zero_action(self, cfg, gripper_cfg, cam_cfg): obs, reward, terminated, truncated, info = env.step(non_zero_action) print(f"{obs=}") """@todo here I expect the final pose to be at least close within 1mm, is this reasonable?""" - assert np.allclose(obs["tquart"], expected_obs, atol=0.001, rtol=0) + out_pose = common.Pose(translation=np.array(obs["tquart"][:3]), quaternion=np.array(obs["tquart"][3:])) + expected_pose = common.Pose(translation=np.array(expected_obs[:3]), + quaternion=np.array(expected_obs[3:])) + assert out_pose.is_close(expected_pose) - @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") + # @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") def test_collision_guard_trpy(self, cfg, gripper_cfg, cam_cfg): """ Check that an obvious collision is detected by the CollisionGuard @@ -162,7 +174,8 @@ def test_collision_guard_trpy(self, cfg, gripper_cfg, cam_cfg): obs_, info_ = env_.reset() print(f"initial_obs: {obs_}") # an obvious below ground collision action - collision_action = OrderedDict([('xyzrpy', np.array([0.3, 0, -0.2, -0.13712998, -2.2763247, + # todo; run this multiple times and notice that sometimes it passes and sometimes it doesnt + collision_action = OrderedDict([('xyzrpy', np.array([0.3, 0, -0.1, -0.13712998, -2.2763247, -0.23976775], dtype=np.float32)), ('gripper', np.array([0.18612409], dtype=np.float32))]) obs, reward, terminated, truncated, info = env_.step(collision_action) @@ -197,7 +210,11 @@ def test_non_zero_action_tquart(self, cfg): print(f"non_zero_action: {non_zero_action}") obs, reward, terminated, truncated, info = env.step(non_zero_action) print(f"{obs=}") - assert np.allclose(obs["tquart"], expected_obs, atol=0.001, rtol=0) + out_pose = common.Pose(translation=np.array(obs["tquart"][:3]), + quaternion=np.array(obs["tquart"][3:])) + expected_pose = common.Pose(translation=np.array(expected_obs[:3]), + quaternion=np.array(expected_obs[3:])) + assert out_pose.is_close(expected_pose) def test_zero_action_tquart(self, cfg): """ @@ -222,16 +239,19 @@ def test_zero_action_tquart(self, cfg): Is it reasonable to use atol=1mm and rtol=0 which means the two values can only vary less than 0.1 to be considered close to each other. """ - assert np.allclose(obs["tquart"], obs_initial["tquart"], atol=0.001, rtol=0) + out_pose = common.Pose(translation=np.array(obs["tquart"][:3]), quaternion=np.array(obs["tquart"][3:])) + expected_pose = common.Pose(translation=np.array(obs_initial["tquart"][:3]), + quaternion=np.array(obs_initial["tquart"][3:])) + assert out_pose.is_close(expected_pose) - @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") + #@pytest.mark.skip(reason="temporarily making it fail due to the TODOs") def test_relative_zero_action_tquart(self, cfg, gripper_cfg, cam_cfg): # env creation env = tf.produce_env_sim_tquart_gripper_camera_rel(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg, gripper_cfg, cam_cfg, robot_id="0") obs_initial, info_ = env.reset() print(f'{obs_initial = }') - zero_rel_action = OrderedDict([('tquart', np.array([0, 0, 0, 0, 0, 0, 0.], dtype=np.float32)), + zero_rel_action = OrderedDict([('tquart', np.array([0, 0, 0, 0, 0, 0, 1.], dtype=np.float32)), ('gripper', np.array([0], dtype=np.float32))]) print(f"zero_rel_action: {zero_rel_action}") obs, reward, terminated, truncated, info = env.step(zero_rel_action) @@ -240,9 +260,12 @@ def test_relative_zero_action_tquart(self, cfg, gripper_cfg, cam_cfg): # @todo # In this test case, a relative action of zeros should not lead to any change in the tcp trpy, but its not # functioning as expected.""" - assert np.allclose(obs["tquart"], obs_initial["tquart"], atol=0.001, rtol=0) + out_pose = common.Pose(translation=np.array(obs["tquart"][:3]), quaternion=np.array(obs["tquart"][3:])) + expected_pose = common.Pose(translation=np.array(obs_initial["tquart"][:3]), + quaternion=np.array(obs_initial["tquart"][3:])) + assert out_pose.is_close(expected_pose) - @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") + # @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") def test_collision_guard_tquart(self, cfg, gripper_cfg, cam_cfg): """ Check that an obvious collision is detected by the CollisionGuard @@ -259,7 +282,7 @@ def test_collision_guard_tquart(self, cfg, gripper_cfg, cam_cfg): obs_, info_ = env_.reset() print(f"initial_obs: {obs_}") # this is a pose with an obvious collision (below the floor) - act = OrderedDict([('tquart', np.array([0.3, 0, -0.5, 9.23879533e-01, + act = OrderedDict([('tquart', np.array([0.3, 0, -0.01, 9.23879533e-01, -3.82683432e-01, -7.25288143e-17, -3.00424186e-17])), ('gripper', np.array([0], dtype=np.float32))]) print(f"act: {act}") @@ -287,9 +310,9 @@ def test_zero_action_joints(self, cfg): print(f"zero_action: {zero_action}") obs, reward, terminated, truncated, info = env_.step(zero_action) print(f"{obs=}") - assert np.allclose(obs["tquart"], obs_initial["tquart"], atol=0.001, rtol=0) + assert np.allclose(obs["joints"], obs_initial["joints"], atol=0.001, rtol=0) - @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") + # @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") def test_non_zero_action_joints(self, cfg): """ This is for testing that a certain action leads to the expected change in state From 696d5f9058872360bb185c04658b997f2fff1111 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 23 Aug 2024 09:56:02 +0200 Subject: [PATCH 18/22] test(typed factory): added check bounds, fixed tests - added check bounds for pose comparisons - fixed join non zero action test by using relative joint pose - formatted typed factory tests - removed print statements and todo comments --- python/tests/test_sim_envs.py | 318 +++++++++++++++++----------------- 1 file changed, 155 insertions(+), 163 deletions(-) diff --git a/python/tests/test_sim_envs.py b/python/tests/test_sim_envs.py index 94ae2a7c..4dbde5bc 100644 --- a/python/tests/test_sim_envs.py +++ b/python/tests/test_sim_envs.py @@ -8,6 +8,12 @@ from rcsss.envs import typed_factory as tf from rcsss import sim +from rcsss._core.sim import CameraType +from rcsss.camera.sim import SimCameraConfig, SimCameraSet +from rcsss.envs.base import CameraSetWrapper, ControlMode, FR3Env, GripperWrapper, RelativeActionSpace +from rcsss.envs.sim import CollisionGuard, FR3Sim +import gymnasium as gym + @pytest.fixture def cfg(): @@ -33,6 +39,7 @@ def cam_cfg(): class TestSimEnvs: """This class is for testing common sim env functionalities""" + mjcf_path = str(rcsss.scenes["fr3_empty_world"]) urdf_path = str(rcsss.scenes["lab"].parent / "fr3.urdf") @@ -45,119 +52,101 @@ def test_model_availability(self): class TestSimEnvsTRPY: """This class is for testing TRPY sim env functionalities""" - # @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") def test_zero_action_trpy(self, cfg): """ Test that a zero action does not change the state significantly """ env, _ = tf.produce_env_sim_trpy(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg, robot_id="0") - obs_initial, info_ = env.reset() - print(f"initial_obs: {obs_initial}") - print(f"info: {info_}") + obs_initial, _ = env.reset() + t = obs_initial["tquart"][:3] q = obs_initial["tquart"][3:] initial_pose = common.Pose(translation=np.array(t), quaternion=np.array(q)) xyzrpy = t.tolist() + initial_pose.rotation_rpy().as_vector().tolist() zero_action = OrderedDict([("xyzrpy", xyzrpy)]) - print(f"zero_action: {zero_action}") - obs, reward, terminated, truncated, info = env.step(zero_action) - print(f"{obs=}") - """ - @todo Assumptions taken, - after env.reset() is called, the robot tcp will be in the home position, and then applying an action - (set_cartesian_position(home_position)) should not change the observation significantly. + obs, _, _, _, info = env.step(zero_action) + assert info["ik_success"] + assert info["is_sim_converged"] - Is the precision of 1 mm reasonable? - """ out_pose = common.Pose(translation=np.array(obs["tquart"][:3]), quaternion=np.array(obs["tquart"][3:])) expected_pose = initial_pose - assert out_pose.is_close(expected_pose) + assert out_pose.is_close(expected_pose, 1e-2, 1e-3) - # @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") def test_non_zero_action_trpy(self, cfg): """ This is for testing that a certain action leads to the expected change in state """ # env creation env, _ = tf.produce_env_sim_trpy(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg, robot_id="0") - obs_initial, info_ = env.reset() - print(f"initial_obs: {obs_initial}") + obs_initial, _ = env.reset() # action to be performed` x_pos_change = 0.2 initial_tquart = obs_initial["tquart"].copy() t = initial_tquart[:3] - # let's shift the translation in x + # shift the translation in x t[0] += x_pos_change - print(f"{t = }") q = initial_tquart[3:] initial_pose = common.Pose(translation=np.array(t), quaternion=np.array(q)) xyzrpy = t.tolist() + initial_pose.rotation_rpy().as_vector().tolist() - non_zero_action = OrderedDict([("xyzrpy", np.array(xyzrpy)), - ('gripper', np.array([0], dtype=np.float32))]) + non_zero_action = OrderedDict([("xyzrpy", np.array(xyzrpy)), ("gripper", np.array([0], dtype=np.float32))]) expected_obs = obs_initial["tquart"].copy() expected_obs[0] += x_pos_change - print(f"expected_obs: {expected_obs}") - print(f"non_zero_action: {non_zero_action}") - obs, reward, terminated, truncated, info = env.step(non_zero_action) - print(f"{obs=}") - """ - @todo the test does not match the expected outputs - """ + obs, _, _, _, info = env.step(non_zero_action) + assert info["ik_success"] + assert info["is_sim_converged"] + out_pose = common.Pose(translation=np.array(obs["tquart"][:3]), quaternion=np.array(obs["tquart"][3:])) - expected_pose = common.Pose(translation=np.array(expected_obs[:3]), - quaternion=np.array(expected_obs[3:])) - assert out_pose.is_close(expected_pose) + expected_pose = common.Pose(translation=np.array(expected_obs[:3]), quaternion=np.array(expected_obs[3:])) + assert out_pose.is_close(expected_pose, 1e-2, 1e-3) def test_relative_zero_action_trpy(self, cfg, gripper_cfg, cam_cfg): # env creation - env = tf.produce_env_sim_trpy_gripper_camera_rel(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, - cfg, gripper_cfg, cam_cfg, robot_id="0") - obs_initial, info_ = env.reset() + env = tf.produce_env_sim_trpy_gripper_camera_rel( + TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg, gripper_cfg, cam_cfg, robot_id="0" + ) + obs_initial, _ = env.reset() - print(f"initial_obs: {obs_initial}") # action to be performed - zero_action = OrderedDict([("xyzrpy", np.array([0, 0, 0, 0, 0, 0], dtype=np.float32)), - ('gripper', np.array([0], dtype=np.float32))]) - print(f"zero_action: {zero_action}") - obs, reward, terminated, truncated, info = env.step(zero_action) - print(f"{obs=}") - print(f"{info=}") - """ - @todo - In this test case, a relative action of zeros should not lead to any change in the tcp trpy, but its not - functioning as expected.""" - # assert np.allclose(obs["tquart"], obs_initial["tquart"], atol=0.1, rtol=0) + zero_action = OrderedDict( + [("xyzrpy", np.array([0, 0, 0, 0, 0, 0], dtype=np.float32)), ("gripper", np.array([0], dtype=np.float32))] + ) + obs, _, _, _, info = env.step(zero_action) + assert info["ik_success"] + assert info["is_sim_converged"] + out_pose = common.Pose(translation=np.array(obs["tquart"][:3]), quaternion=np.array(obs["tquart"][3:])) - expected_pose = common.Pose(translation=np.array(obs_initial["tquart"][:3]), - quaternion=np.array(obs_initial["tquart"][3:])) - assert out_pose.is_close(expected_pose) + expected_pose = common.Pose( + translation=np.array(obs_initial["tquart"][:3]), quaternion=np.array(obs_initial["tquart"][3:]) + ) + assert out_pose.is_close(expected_pose, 1e-2, 1e-3) - # @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") def test_relative_non_zero_action(self, cfg, gripper_cfg, cam_cfg): # env creation - env = tf.produce_env_sim_trpy_gripper_camera_cg_rel(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, - cfg, gripper_cfg, cam_cfg, robot_id="0") - obs_initial, info_ = env.reset() + env = tf.produce_env_sim_trpy_gripper_camera_cg_rel( + TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg, gripper_cfg, cam_cfg, robot_id="0" + ) + obs_initial, _ = env.reset() - print(f"initial_obs: {obs_initial}") # action to be performed x_pos_change = 0.2 - non_zero_action = OrderedDict([("xyzrpy", np.array([x_pos_change, 0, 0, 0, 0, 0], dtype=np.float32)), - ('gripper', np.array([0], dtype=np.float32))]) + non_zero_action = OrderedDict( + [ + ("xyzrpy", np.array([x_pos_change, 0, 0, 0, 0, 0], dtype=np.float32)), + ("gripper", np.array([0], dtype=np.float32)), + ] + ) expected_obs = obs_initial["tquart"].copy() expected_obs[0] += x_pos_change - print(f"expected_obs: {expected_obs}") - print(f"non_zero_action: {non_zero_action}") obs, reward, terminated, truncated, info = env.step(non_zero_action) - print(f"{obs=}") - """@todo here I expect the final pose to be at least close within 1mm, is this reasonable?""" + assert info["ik_success"] + assert info["is_sim_converged"] + out_pose = common.Pose(translation=np.array(obs["tquart"][:3]), quaternion=np.array(obs["tquart"][3:])) - expected_pose = common.Pose(translation=np.array(expected_obs[:3]), - quaternion=np.array(expected_obs[3:])) - assert out_pose.is_close(expected_pose) + expected_pose = common.Pose(translation=np.array(expected_obs[:3]), quaternion=np.array(expected_obs[3:])) + assert out_pose.is_close(expected_pose, 1e-2, 1e-3) - # @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") + # seems to make problems sometimes def test_collision_guard_trpy(self, cfg, gripper_cfg, cam_cfg): """ Check that an obvious collision is detected by the CollisionGuard @@ -171,23 +160,24 @@ def test_collision_guard_trpy(self, cfg, gripper_cfg, cam_cfg): cam_cfg=cam_cfg, robot_id="0", ) - obs_, info_ = env_.reset() - print(f"initial_obs: {obs_}") + env_.reset() # an obvious below ground collision action - # todo; run this multiple times and notice that sometimes it passes and sometimes it doesnt - collision_action = OrderedDict([('xyzrpy', np.array([0.3, 0, -0.1, -0.13712998, -2.2763247, - -0.23976775], dtype=np.float32)), - ('gripper', np.array([0.18612409], dtype=np.float32))]) - obs, reward, terminated, truncated, info = env_.step(collision_action) - print(f'{obs = }') - print(f"info: {info}") - # """This is a scenario of the tcp below the ground which is an obvious collision, however the info.collision - # property is not populated correctly""" + # TODO: run this multiple times and notice that sometimes it passes and sometimes it doesnt + collision_action = OrderedDict( + [ + ("xyzrpy", np.array([0.3, 0, -0.1, -0.13712998, -2.2763247, -0.23976775], dtype=np.float32)), + ("gripper", np.array([0.18612409], dtype=np.float32)), + ] + ) + _, _, _, _, info = env_.step(collision_action) + # This is a scenario of the tcp below the ground which is an obvious collision, however the info.collision + # property is not populated correctly + + assert info["ik_success"] assert info["collision"] class TestSimEnvsTquart: - """This class is for testing Tquart sim env functionalities""" def test_non_zero_action_tquart(self, cfg): @@ -195,77 +185,67 @@ def test_non_zero_action_tquart(self, cfg): Test that a zero action does not change the state significantly in the tquart configuration """ env, _ = tf.produce_env_sim_tquart(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg, robot_id="0") - obs_initial, info_ = env.reset() - print(f"initial_obs: {obs_initial}") + obs_initial, _ = env.reset() + # action to be performed t = obs_initial["tquart"][:3].tolist() q = obs_initial["tquart"][3:].tolist() x_pos_change = 0.3 # updating the x action by 20cm t[0] += x_pos_change - non_zero_action = OrderedDict([('tquart', np.array(t + q, dtype=np.float32))]) + non_zero_action = OrderedDict([("tquart", np.array(t + q, dtype=np.float32))]) expected_obs = obs_initial["tquart"].copy() expected_obs[0] += x_pos_change - print(f"non_zero_action: {non_zero_action}") - obs, reward, terminated, truncated, info = env.step(non_zero_action) - print(f"{obs=}") - out_pose = common.Pose(translation=np.array(obs["tquart"][:3]), - quaternion=np.array(obs["tquart"][3:])) - expected_pose = common.Pose(translation=np.array(expected_obs[:3]), - quaternion=np.array(expected_obs[3:])) - assert out_pose.is_close(expected_pose) + obs, _, _, _, info = env.step(non_zero_action) + out_pose = common.Pose(translation=np.array(obs["tquart"][:3]), quaternion=np.array(obs["tquart"][3:])) + expected_pose = common.Pose(translation=np.array(expected_obs[:3]), quaternion=np.array(expected_obs[3:])) + assert info["ik_success"] + assert out_pose.is_close(expected_pose, 1e-2, 1e-3) + + # @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") def test_zero_action_tquart(self, cfg): """ Test that a zero action does not change the state significantly in the tquart configuration """ env, _ = tf.produce_env_sim_tquart(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg, robot_id="0") obs_initial, info_ = env.reset() - print(f"initial_obs: {obs_initial}") - print(f"info: {info_}") + home_action_vec = obs_initial["tquart"] zero_action = OrderedDict([("tquart", home_action_vec)]) - print(f"zero_action: {zero_action}") - obs, reward, terminated, truncated, info = env.step(zero_action) - print(f"{obs=}") - """ - @todo Assumptions taken, - after env.reset() is called, the robot tcp will be in the home position, and then applying an action - (set_cartesian_position(home_position)) should not change the observation. - - But the observation doesnt match ...??? - Is it reasonable to use atol=1mm and rtol=0 which means the two values can only vary less than 0.1 to be - considered close to each other. - """ + obs, _, _, _, info = env.step(zero_action) + assert info["ik_success"] + assert info["is_sim_converged"] out_pose = common.Pose(translation=np.array(obs["tquart"][:3]), quaternion=np.array(obs["tquart"][3:])) - expected_pose = common.Pose(translation=np.array(obs_initial["tquart"][:3]), - quaternion=np.array(obs_initial["tquart"][3:])) - assert out_pose.is_close(expected_pose) + expected_pose = common.Pose( + translation=np.array(obs_initial["tquart"][:3]), quaternion=np.array(obs_initial["tquart"][3:]) + ) + assert out_pose.is_close(expected_pose, 1e-2, 1e-3) - #@pytest.mark.skip(reason="temporarily making it fail due to the TODOs") def test_relative_zero_action_tquart(self, cfg, gripper_cfg, cam_cfg): # env creation - env = tf.produce_env_sim_tquart_gripper_camera_rel(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, - cfg, gripper_cfg, cam_cfg, robot_id="0") - obs_initial, info_ = env.reset() - print(f'{obs_initial = }') - zero_rel_action = OrderedDict([('tquart', np.array([0, 0, 0, 0, 0, 0, 1.], dtype=np.float32)), - ('gripper', np.array([0], dtype=np.float32))]) - print(f"zero_rel_action: {zero_rel_action}") - obs, reward, terminated, truncated, info = env.step(zero_rel_action) - print(f"{obs=}") - # """ - # @todo - # In this test case, a relative action of zeros should not lead to any change in the tcp trpy, but its not - # functioning as expected.""" + env = tf.produce_env_sim_tquart_gripper_camera_rel( + TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg, gripper_cfg, cam_cfg, robot_id="0" + ) + obs_initial, _ = env.reset() + print(f"{obs_initial = }") + zero_rel_action = OrderedDict( + [ + ("tquart", np.array([0, 0, 0, 0, 0, 0, 1.0], dtype=np.float32)), + ("gripper", np.array([0], dtype=np.float32)), + ] + ) + obs, _, _, _, info = env.step(zero_rel_action) + assert info["ik_success"] + assert info["is_sim_converged"] out_pose = common.Pose(translation=np.array(obs["tquart"][:3]), quaternion=np.array(obs["tquart"][3:])) - expected_pose = common.Pose(translation=np.array(obs_initial["tquart"][:3]), - quaternion=np.array(obs_initial["tquart"][3:])) - assert out_pose.is_close(expected_pose) + expected_pose = common.Pose( + translation=np.array(obs_initial["tquart"][:3]), quaternion=np.array(obs_initial["tquart"][3:]) + ) + assert out_pose.is_close(expected_pose, 1e-2, 1e-3) - # @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") def test_collision_guard_tquart(self, cfg, gripper_cfg, cam_cfg): """ Check that an obvious collision is detected by the CollisionGuard @@ -279,54 +259,55 @@ def test_collision_guard_tquart(self, cfg, gripper_cfg, cam_cfg): cam_cfg=cam_cfg, robot_id="0", ) - obs_, info_ = env_.reset() - print(f"initial_obs: {obs_}") + env_.reset() # this is a pose with an obvious collision (below the floor) - act = OrderedDict([('tquart', np.array([0.3, 0, -0.01, 9.23879533e-01, - -3.82683432e-01, -7.25288143e-17, -3.00424186e-17])), - ('gripper', np.array([0], dtype=np.float32))]) - print(f"act: {act}") - obs, reward, terminated, truncated, info = env_.step(act) - print(f'{obs = }') - print(f"info: {info}") - """ - @todo - This is a scenario of the tcp below the ground which is an obvious collision, however the info.collision - property is not populated collision_action""" + act = OrderedDict( + [ + ( + "tquart", + np.array([0.3, 0, -0.01, 9.23879533e-01, -3.82683432e-01, -7.25288143e-17, -3.00424186e-17]), + ), + ("gripper", np.array([0], dtype=np.float32)), + ] + ) + _, _, _, _, info = env_.step(act) + + assert info["ik_success"] assert info["collision"] class TestSimEnvsJoints: """This class is for testing Joints sim env functionalities""" + def test_zero_action_joints(self, cfg): """ This is for testing that a certain action leads to the expected change in state """ env_, _ = tf.produce_env_sim_joints(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg, robot_id="0") - obs_initial, info_ = env_.reset() - print(f"initial_obs: {obs_initial}") + obs_initial, _ = env_.reset() + # action to be performed - zero_action = OrderedDict([('joints', np.array(obs_initial['joints']))]) - print(f"zero_action: {zero_action}") - obs, reward, terminated, truncated, info = env_.step(zero_action) - print(f"{obs=}") - assert np.allclose(obs["joints"], obs_initial["joints"], atol=0.001, rtol=0) + zero_action = OrderedDict([("joints", np.array(obs_initial["joints"]))]) + obs, _, _, _, info = env_.step(zero_action) + assert info["ik_success"] + assert info["is_sim_converged"] + assert np.allclose(obs["joints"], obs_initial["joints"], atol=0.01, rtol=0) - # @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") def test_non_zero_action_joints(self, cfg): """ This is for testing that a certain action leads to the expected change in state """ env_, _ = tf.produce_env_sim_joints(TestSimEnvs.mjcf_path, TestSimEnvs.urdf_path, cfg, robot_id="0") - obs_initial, info_ = env_.reset() - print(f"initial_obs: {obs_initial}") + obs_initial, _ = env_.reset() # action to be performed - non_zero_action = OrderedDict([('joints', np.array([0, 0.731, 0, 0, 0, 0, 0]))]) - print(f"non_zero_action: {non_zero_action}") - obs, reward, terminated, truncated, info = env_.step(non_zero_action) - print(f"{obs=}") - """@todo, If i set a particular joint angle, I assume that the observed joint angle must be the same""" - assert np.allclose(obs["joints"], non_zero_action["joints"], atol=0.001, rtol=0) + non_zero_action = OrderedDict( + [("joints", obs_initial["joints"] + np.array([0.1, 0.1, 0.1, 0.1, -0.1, -0.1, 0.1]))] + ) + obs, _, _, _, info = env_.step(non_zero_action) + assert info["ik_success"] + assert info["is_sim_converged"] + + assert np.allclose(obs["joints"], non_zero_action["joints"], atol=0.01, rtol=0) def test_collision_guard_joints(self, cfg, gripper_cfg, cam_cfg): """ @@ -341,12 +322,15 @@ def test_collision_guard_joints(self, cfg, gripper_cfg, cam_cfg): cam_cfg=cam_cfg, robot_id="0", ) - obs_, info_ = env_.reset() - print(f"initial_obs: {obs_}") + env_.reset() # the below action is a test_case where there is an obvious collision regardless of the gripper action - act = OrderedDict([('joints', np.array([0, 1.78, 0, -1.45, 0, 0, 0], dtype=np.float32)), - ('gripper', np.array([0.5209108], dtype=np.float32))]) - obs, reward, terminated, truncated, info = env_.step(act) + act = OrderedDict( + [ + ("joints", np.array([0, 1.78, 0, -1.45, 0, 0, 0], dtype=np.float32)), + ("gripper", np.array([0.5209108], dtype=np.float32)), + ] + ) + _, _, _, _, info = env_.step(act) assert info["collision"] def test_relative_zero_action_joints(self, cfg, gripper_cfg, cam_cfg): @@ -362,10 +346,18 @@ def test_relative_zero_action_joints(self, cfg, gripper_cfg, cam_cfg): cam_cfg=cam_cfg, robot_id="0", ) - obs_initial, info_ = env_.reset() - print(f"initial_obs: {obs_initial}") - act = OrderedDict([('joints', np.array([0, 0, 0, 0, 0, 0, 0], dtype=np.float32)), - ('gripper', np.array([0], dtype=np.float32))]) - print(f'{act = }') - obs, reward, terminated, truncated, info = env_.step(act) - assert np.allclose(obs["tquart"], obs_initial["tquart"], atol=0.001, rtol=0) + obs_initial, _ = env_.reset() + act = OrderedDict( + [ + ("joints", np.array([0, 0, 0, 0, 0, 0, 0], dtype=np.float32)), + ("gripper", np.array([0], dtype=np.float32)), + ] + ) + obs, _, _, _, info = env_.step(act) + assert info["ik_success"] + assert info["is_sim_converged"] + out_pose = common.Pose(translation=np.array(obs["tquart"][:3]), quaternion=np.array(obs["tquart"][3:])) + expected_pose = common.Pose( + translation=np.array(obs_initial["tquart"][:3]), quaternion=np.array(obs_initial["tquart"][3:]) + ) + assert out_pose.is_close(expected_pose, 1e-2, 1e-3) From 623fddf33b7ece6741276bb30cb5bcdad1de21f1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 23 Aug 2024 11:22:13 +0200 Subject: [PATCH 19/22] feat(env): added trpy as observation to fr3 env --- python/rcsss/envs/base.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/python/rcsss/envs/base.py b/python/rcsss/envs/base.py index 94c0cc06..51b6bb7e 100644 --- a/python/rcsss/envs/base.py +++ b/python/rcsss/envs/base.py @@ -111,8 +111,8 @@ class CameraDictType(RCSpaceType): ] -# joining works with inhertiance but need to inherit from protocol again -class ArmObsType(TQuartDictType, JointsDictType): ... +# joining works with inheritance but need to inherit from protocol again +class ArmObsType(TQuartDictType, JointsDictType, TRPYDictType): ... CartOrJointContType: TypeAlias = TQuartDictType | JointsDictType | TRPYDictType @@ -191,6 +191,7 @@ def get_obs(self) -> ArmObsType: [self.robot.get_cartesian_position().translation(), self.robot.get_cartesian_position().rotation_q()] ), joints=self.robot.get_joint_position(), + xyzrpy=self.robot.get_cartesian_position().xyzrpy(), ) def step(self, action: CartOrJointContType) -> tuple[ArmObsType, float, bool, bool, dict]: From 2f443bb325f1ba0ec7a7551a75ef1a832cd5ee97 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 23 Aug 2024 11:39:00 +0200 Subject: [PATCH 20/22] test: fix collision pose of rpy collision test - using relative pose with xyzrpy to ensure collision - added is_sim_converged assert - correct type for gripper --- python/tests/test_sim_envs.py | 26 ++++++++++++++++---------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/python/tests/test_sim_envs.py b/python/tests/test_sim_envs.py index 4dbde5bc..9c252459 100644 --- a/python/tests/test_sim_envs.py +++ b/python/tests/test_sim_envs.py @@ -88,7 +88,7 @@ def test_non_zero_action_trpy(self, cfg): q = initial_tquart[3:] initial_pose = common.Pose(translation=np.array(t), quaternion=np.array(q)) xyzrpy = t.tolist() + initial_pose.rotation_rpy().as_vector().tolist() - non_zero_action = OrderedDict([("xyzrpy", np.array(xyzrpy)), ("gripper", np.array([0], dtype=np.float32))]) + non_zero_action = OrderedDict([("xyzrpy", np.array(xyzrpy)), ("gripper", 0)]) expected_obs = obs_initial["tquart"].copy() expected_obs[0] += x_pos_change obs, _, _, _, info = env.step(non_zero_action) @@ -108,7 +108,7 @@ def test_relative_zero_action_trpy(self, cfg, gripper_cfg, cam_cfg): # action to be performed zero_action = OrderedDict( - [("xyzrpy", np.array([0, 0, 0, 0, 0, 0], dtype=np.float32)), ("gripper", np.array([0], dtype=np.float32))] + [("xyzrpy", np.array([0, 0, 0, 0, 0, 0], dtype=np.float32)), ("gripper", 0)] ) obs, _, _, _, info = env.step(zero_action) assert info["ik_success"] @@ -132,7 +132,7 @@ def test_relative_non_zero_action(self, cfg, gripper_cfg, cam_cfg): non_zero_action = OrderedDict( [ ("xyzrpy", np.array([x_pos_change, 0, 0, 0, 0, 0], dtype=np.float32)), - ("gripper", np.array([0], dtype=np.float32)), + ("gripper", 0), ] ) expected_obs = obs_initial["tquart"].copy() @@ -160,13 +160,15 @@ def test_collision_guard_trpy(self, cfg, gripper_cfg, cam_cfg): cam_cfg=cam_cfg, robot_id="0", ) - env_.reset() + obs, _ = env_.reset() # an obvious below ground collision action # TODO: run this multiple times and notice that sometimes it passes and sometimes it doesnt + obs["xyzrpy"][2] = -0.2 + collision_action = OrderedDict( [ - ("xyzrpy", np.array([0.3, 0, -0.1, -0.13712998, -2.2763247, -0.23976775], dtype=np.float32)), - ("gripper", np.array([0.18612409], dtype=np.float32)), + ("xyzrpy", obs["xyzrpy"]), + ("gripper", 0), ] ) _, _, _, _, info = env_.step(collision_action) @@ -174,6 +176,7 @@ def test_collision_guard_trpy(self, cfg, gripper_cfg, cam_cfg): # property is not populated correctly assert info["ik_success"] + assert info["is_sim_converged"] assert info["collision"] @@ -234,7 +237,7 @@ def test_relative_zero_action_tquart(self, cfg, gripper_cfg, cam_cfg): zero_rel_action = OrderedDict( [ ("tquart", np.array([0, 0, 0, 0, 0, 0, 1.0], dtype=np.float32)), - ("gripper", np.array([0], dtype=np.float32)), + ("gripper", 0), ] ) obs, _, _, _, info = env.step(zero_rel_action) @@ -267,12 +270,13 @@ def test_collision_guard_tquart(self, cfg, gripper_cfg, cam_cfg): "tquart", np.array([0.3, 0, -0.01, 9.23879533e-01, -3.82683432e-01, -7.25288143e-17, -3.00424186e-17]), ), - ("gripper", np.array([0], dtype=np.float32)), + ("gripper", 0), ] ) _, _, _, _, info = env_.step(act) assert info["ik_success"] + assert info["is_sim_converged"] assert info["collision"] @@ -327,10 +331,12 @@ def test_collision_guard_joints(self, cfg, gripper_cfg, cam_cfg): act = OrderedDict( [ ("joints", np.array([0, 1.78, 0, -1.45, 0, 0, 0], dtype=np.float32)), - ("gripper", np.array([0.5209108], dtype=np.float32)), + ("gripper", 1), ] ) _, _, _, _, info = env_.step(act) + assert info["ik_success"] + assert info["is_sim_converged"] assert info["collision"] def test_relative_zero_action_joints(self, cfg, gripper_cfg, cam_cfg): @@ -350,7 +356,7 @@ def test_relative_zero_action_joints(self, cfg, gripper_cfg, cam_cfg): act = OrderedDict( [ ("joints", np.array([0, 0, 0, 0, 0, 0, 0], dtype=np.float32)), - ("gripper", np.array([0], dtype=np.float32)), + ("gripper", 0), ] ) obs, _, _, _, info = env_.step(act) From beb845fde0a6ceba58b2ccfd669d3908230085dd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 23 Aug 2024 11:39:18 +0200 Subject: [PATCH 21/22] ci: added xvfb to execute sim tests --- .github/workflows/py.yaml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.github/workflows/py.yaml b/.github/workflows/py.yaml index d796598c..35c52c9e 100644 --- a/.github/workflows/py.yaml +++ b/.github/workflows/py.yaml @@ -32,6 +32,8 @@ jobs: - uses: actions/checkout@v4 - name: Install CPP dependencies run: sudo apt install $(cat debian_deps.txt) + - name: Install virtual frame buffer tool + run: sudo apt install xvfb - name: Set up Python 3.11 uses: actions/setup-python@v5 with: @@ -52,4 +54,4 @@ jobs: - name: Check that stub files are up-to-date run: make stubgen && git diff --exit-code - name: Ensure all unittests(pytest) are passing - run: make pytest + run: xvfb-run make pytest From 4115a32e9c8c7f62ae2f7af3fb2a496f526ae95b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 23 Aug 2024 16:05:36 +0200 Subject: [PATCH 22/22] test: fixed imports, still random fails --- python/tests/test_common.py | 2 +- python/tests/test_envs.py | 3 +-- python/tests/test_sim_envs.py | 25 ++++++++----------------- 3 files changed, 10 insertions(+), 20 deletions(-) diff --git a/python/tests/test_common.py b/python/tests/test_common.py index c246a885..2a241278 100644 --- a/python/tests/test_common.py +++ b/python/tests/test_common.py @@ -1,8 +1,8 @@ import math +import numpy as np import pytest from rcsss import common -import numpy as np class TestPose: diff --git a/python/tests/test_envs.py b/python/tests/test_envs.py index 15fff55d..a65fb808 100644 --- a/python/tests/test_envs.py +++ b/python/tests/test_envs.py @@ -1,8 +1,7 @@ from typing import Annotated -import numpy as np import gymnasium as gym - +import numpy as np from rcsss.envs.space_utils import RCSpaceType, get_space, get_space_keys diff --git a/python/tests/test_sim_envs.py b/python/tests/test_sim_envs.py index 9c252459..b9c757ba 100644 --- a/python/tests/test_sim_envs.py +++ b/python/tests/test_sim_envs.py @@ -1,18 +1,12 @@ -import pytest from collections import OrderedDict -import numpy as np -from rcsss._core import common +import numpy as np +import pytest import rcsss +from rcsss import sim +from rcsss._core import common from rcsss.camera.sim import SimCameraSetConfig from rcsss.envs import typed_factory as tf -from rcsss import sim - -from rcsss._core.sim import CameraType -from rcsss.camera.sim import SimCameraConfig, SimCameraSet -from rcsss.envs.base import CameraSetWrapper, ControlMode, FR3Env, GripperWrapper, RelativeActionSpace -from rcsss.envs.sim import CollisionGuard, FR3Sim -import gymnasium as gym @pytest.fixture @@ -107,9 +101,7 @@ def test_relative_zero_action_trpy(self, cfg, gripper_cfg, cam_cfg): obs_initial, _ = env.reset() # action to be performed - zero_action = OrderedDict( - [("xyzrpy", np.array([0, 0, 0, 0, 0, 0], dtype=np.float32)), ("gripper", 0)] - ) + zero_action = OrderedDict([("xyzrpy", np.array([0, 0, 0, 0, 0, 0], dtype=np.float32)), ("gripper", 0)]) obs, _, _, _, info = env.step(zero_action) assert info["ik_success"] assert info["is_sim_converged"] @@ -138,7 +130,7 @@ def test_relative_non_zero_action(self, cfg, gripper_cfg, cam_cfg): expected_obs = obs_initial["tquart"].copy() expected_obs[0] += x_pos_change - obs, reward, terminated, truncated, info = env.step(non_zero_action) + obs, _, _, _, info = env.step(non_zero_action) assert info["ik_success"] assert info["is_sim_converged"] @@ -146,7 +138,7 @@ def test_relative_non_zero_action(self, cfg, gripper_cfg, cam_cfg): expected_pose = common.Pose(translation=np.array(expected_obs[:3]), quaternion=np.array(expected_obs[3:])) assert out_pose.is_close(expected_pose, 1e-2, 1e-3) - # seems to make problems sometimes + # TODO: collision test still randomly fails def test_collision_guard_trpy(self, cfg, gripper_cfg, cam_cfg): """ Check that an obvious collision is detected by the CollisionGuard @@ -162,7 +154,6 @@ def test_collision_guard_trpy(self, cfg, gripper_cfg, cam_cfg): ) obs, _ = env_.reset() # an obvious below ground collision action - # TODO: run this multiple times and notice that sometimes it passes and sometimes it doesnt obs["xyzrpy"][2] = -0.2 collision_action = OrderedDict( @@ -207,7 +198,6 @@ def test_non_zero_action_tquart(self, cfg): assert info["ik_success"] assert out_pose.is_close(expected_pose, 1e-2, 1e-3) - # @pytest.mark.skip(reason="temporarily making it fail due to the TODOs") def test_zero_action_tquart(self, cfg): """ Test that a zero action does not change the state significantly in the tquart configuration @@ -249,6 +239,7 @@ def test_relative_zero_action_tquart(self, cfg, gripper_cfg, cam_cfg): ) assert out_pose.is_close(expected_pose, 1e-2, 1e-3) + # TODO: collision test still randomly fails def test_collision_guard_tquart(self, cfg, gripper_cfg, cam_cfg): """ Check that an obvious collision is detected by the CollisionGuard