diff --git a/pyproject.toml b/pyproject.toml index 29c4414e..8f5b7664 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -19,6 +19,7 @@ dependencies = ["websockets>=11.0", "pyopengl", "pyrealsense2~=2.55.1", "pillow~=10.3", + "python-dotenv==1.0.1", # NOTE: when changing the mujoco version, also change it in requirements_dev.txt "mujoco==3.1.5" ] diff --git a/python/examples/cartesian_control.py b/python/examples/env_cartesian_control.py similarity index 70% rename from python/examples/cartesian_control.py rename to python/examples/env_cartesian_control.py index 24495a51..989c90ed 100644 --- a/python/examples/cartesian_control.py +++ b/python/examples/env_cartesian_control.py @@ -4,7 +4,7 @@ from rcsss.desk import FCI, Desk, DummyResourceManager from rcsss.envs.base import ControlMode, RobotInstance -from common import hw_env_rel, sim_env_rel +from env_common import hw_env_rel, sim_env_rel logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -22,26 +22,31 @@ def main(): if ROBOT_INSTANCE == RobotInstance.HARDWARE: - env_rel = hw_env_rel(ROBOT_IP, ControlMode.CARTESIAN_TQuart) creds = dotenv_values() resource_manger = FCI( - Desk(ROBOT_IP, creds["FR3_USERNAME"], creds["FR3_USERNAME"]), unlock=False, lock_when_done=False + Desk(ROBOT_IP, creds["FR3_USERNAME"], creds["FR3_PASSWORD"]), unlock=False, lock_when_done=False ) else: - env_rel = sim_env_rel(ControlMode.CARTESIAN_TQuart) resource_manger = DummyResourceManager() - print(env_rel.unwrapped.robot.get_cartesian_position()) - with resource_manger: + if ROBOT_INSTANCE == RobotInstance.HARDWARE: + env_rel = hw_env_rel(ROBOT_IP, ControlMode.CARTESIAN_TQuart) + else: + env_rel = sim_env_rel(ControlMode.CARTESIAN_TQuart) + + print(env_rel.unwrapped.robot.get_cartesian_position()) + for _ in range(10): for _ in range(10): + # move 1cm in x direction (forward) and close gripper act = {"tquart": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 0} obs, reward, terminated, truncated, info = env_rel.step(act) if truncated or terminated: logger.info("Truncated or terminated!") return for _ in range(10): + # move 1cm in negative x direction (backward) and open gripper act = {"tquart": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": 1} obs, reward, terminated, truncated, info = env_rel.step(act) if truncated or terminated: diff --git a/python/examples/common.py b/python/examples/env_common.py similarity index 97% rename from python/examples/common.py rename to python/examples/env_common.py index b9fe99e9..12281552 100644 --- a/python/examples/common.py +++ b/python/examples/env_common.py @@ -31,6 +31,7 @@ def hw_env_rel(ip: str, control_mode: ControlMode) -> gym.Env[ObsArmsGr, Limited sys.exit() robot = rcsss.hw.FR3(ip, str(rcsss.scenes["lab"].parent / "fr3.urdf")) robot_cfg = FR3Config() + robot_cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) robot.set_parameters(robot_cfg) # the robot itself is always controlled in joint space with the robotics library IK env = FR3Env(robot, ControlMode.JOINTS) @@ -38,7 +39,7 @@ def hw_env_rel(ip: str, control_mode: ControlMode) -> gym.Env[ObsArmsGr, Limited gripper_cfg = rcsss.hw.FHConfig() gripper_cfg.epsilon_inner = gripper_cfg.epsilon_outer = 0.1 gripper_cfg.speed = 0.1 - gripper_cfg.force = 10 + gripper_cfg.force = 30 gripper = rcsss.hw.FrankaHand(ip, gripper_cfg) env_hw = GripperWrapper(env_hw, gripper, binary=True) diff --git a/python/examples/joint_control.py b/python/examples/env_joint_control.py similarity index 66% rename from python/examples/joint_control.py rename to python/examples/env_joint_control.py index fba3f7b8..85e3ff40 100644 --- a/python/examples/joint_control.py +++ b/python/examples/env_joint_control.py @@ -1,10 +1,12 @@ import logging +import mujoco +import rcsss from dotenv import dotenv_values from rcsss.desk import FCI, Desk, DummyResourceManager from rcsss.envs.base import ControlMode, RobotInstance -from common import hw_env_rel, sim_env_rel +from env_common import hw_env_rel, sim_env_rel logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -22,20 +24,26 @@ def main(): if ROBOT_INSTANCE == RobotInstance.HARDWARE: - env_rel = hw_env_rel(ROBOT_IP, ControlMode.JOINTS) creds = dotenv_values() resource_manger = FCI( - Desk(ROBOT_IP, creds["FR3_USERNAME"], creds["FR3_USERNAME"]), unlock=False, lock_when_done=False + Desk(ROBOT_IP, creds["FR3_USERNAME"], creds["FR3_PASSWORD"]), unlock=False, lock_when_done=False ) else: - env_rel = sim_env_rel(ControlMode.JOINTS) resource_manger = DummyResourceManager() - with resource_manger: + + if ROBOT_INSTANCE == RobotInstance.HARDWARE: + env_rel = hw_env_rel(ROBOT_IP, ControlMode.JOINTS) + else: + env_rel = sim_env_rel(ControlMode.JOINTS) + for _ in range(10): obs, info = env_rel.reset() - for _ in range(10): + for _ in range(3): + # sample random relative action and execute it act = env_rel.action_space.sample() + # if the first is open, then it does not open + act["gripper"] = 1 obs, reward, terminated, truncated, info = env_rel.step(act) if truncated or terminated: logger.info("Truncated or terminated!") diff --git a/python/examples/fr3.py b/python/examples/fr3.py new file mode 100644 index 00000000..12744efd --- /dev/null +++ b/python/examples/fr3.py @@ -0,0 +1,178 @@ +import logging +import sys + +import numpy as np +import rcsss +from dotenv import dotenv_values +from rcsss import sim +from rcsss._core.hw import FR3Config, IKController +from rcsss._core.sim import CameraType +from rcsss.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig +from rcsss.desk import FCI, Desk, DummyResourceManager +from rcsss.envs.base import RobotInstance + +ROBOT_IP = "192.168.101.1" +ROBOT_INSTANCE = RobotInstance.SIMULATION + +logger = logging.getLogger(__name__) +logger.setLevel(logging.DEBUG) +logger.addHandler(logging.StreamHandler()) + +""" +This examples demonstrates the direct robot control api without gym env. + +First the robot is moved in x, y and z direction 5cm. Then the arm is rotated to the right +and trying to grasp an object placed 25cm to the right of it. Afterwards it moves back +to the home position. + + +Create a .env file in the same directory as this file with the following content: +FR3_USERNAME= +FR3_PASSWORD= +""" + + +def main(): + if "lab" not in rcsss.scenes: + logger.error("This pip package was not built with the UTN lab models, aborting.") + sys.exit() + if ROBOT_INSTANCE == RobotInstance.HARDWARE: + creds = dotenv_values() + resource_manger = FCI( + Desk(ROBOT_IP, creds["FR3_USERNAME"], creds["FR3_PASSWORD"]), unlock=False, lock_when_done=False + ) + else: + resource_manger = DummyResourceManager() + + + with resource_manger: + if ROBOT_INSTANCE == RobotInstance.SIMULATION: + simulation = sim.Sim(rcsss.scenes["fr3_empty_world"]) + robot = rcsss.sim.FR3(simulation, "0", str(rcsss.scenes["lab"].parent / "fr3.urdf")) + cfg = sim.FR3Config() + cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) + cfg.ik_duration_in_milliseconds = 300 + cfg.realtime = False + robot.set_parameters(cfg) + + gripper_cfg = sim.FHConfig() + gripper = sim.FrankaHand(simulation, "0", gripper_cfg) + + # add camera to have a rendering gui + cameras = { + "default_free": SimCameraConfig(identifier="", type=int(CameraType.default_free), on_screen_render=True), + "wrist": SimCameraConfig(identifier="eye-in-hand_0", type=int(CameraType.fixed), on_screen_render=False), + # TODO: odd behavior when not both cameras are used: only last image is shown + } + cam_cfg = SimCameraSetConfig(cameras=cameras, resolution_width=1280, resolution_height=720, frame_rate=20) + camera_set = SimCameraSet(simulation, cam_cfg) + resource_manger = DummyResourceManager() + + else: + robot = rcsss.hw.FR3(ROBOT_IP, str(rcsss.scenes["lab"].parent / "fr3.urdf")) + robot_cfg = FR3Config() + robot_cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) + robot_cfg.controller = IKController.robotics_library + robot.set_parameters(robot_cfg) + + gripper_cfg = rcsss.hw.FHConfig() + gripper_cfg.epsilon_inner = gripper_cfg.epsilon_outer = 0.1 + gripper_cfg.speed = 0.1 + gripper_cfg.force = 30 + gripper = rcsss.hw.FrankaHand(ROBOT_IP, gripper_cfg) + creds = dotenv_values() + resource_manger = FCI( + Desk(ROBOT_IP, creds["FR3_USERNAME"], creds["FR3_PASSWORD"]), unlock=False, lock_when_done=False + ) + input("the robot is going to move, press enter whenever you are ready") + + + # move to home position and open gripper + robot.move_home() + gripper.open() + if ROBOT_INSTANCE == RobotInstance.SIMULATION: + simulation.step_until_convergence() + logger.info("Robot is in home position, gripper is open") + + # 5cm in x direction + robot.set_cartesian_position( + robot.get_cartesian_position() * rcsss.common.Pose(translation=np.array([0.05, 0, 0])) + ) + if ROBOT_INSTANCE == RobotInstance.SIMULATION: + simulation.step_until_convergence() + logger.debug(f"IK success: {robot.get_state().ik_success}") + logger.debug(f"sim converged: {simulation.is_converged()}") + + # 5cm in y direction + robot.set_cartesian_position( + robot.get_cartesian_position() * rcsss.common.Pose(translation=np.array([0, 0.05, 0])) + ) + if ROBOT_INSTANCE == RobotInstance.SIMULATION: + simulation.step_until_convergence() + logger.debug(f"IK success: {robot.get_state().ik_success}") + logger.debug(f"sim converged: {simulation.is_converged()}") + + # 5cm in z direction + robot.set_cartesian_position( + robot.get_cartesian_position() * rcsss.common.Pose(translation=np.array([0, 0, 0.05])) + ) + if ROBOT_INSTANCE == RobotInstance.SIMULATION: + simulation.step_until_convergence() + logger.debug(f"IK success: {robot.get_state().ik_success}") + logger.debug(f"sim converged: {simulation.is_converged()}") + + # rotate the arm 90 degrees around the inverted y and z axis + new_pose = robot.get_cartesian_position() * rcsss.common.Pose( + translation=np.array([0, 0, 0]), rpy=rcsss.common.RPY(roll=0, pitch=-np.deg2rad(90), yaw=-np.deg2rad(90)) + ) + robot.set_cartesian_position(new_pose) + if ROBOT_INSTANCE == RobotInstance.SIMULATION: + simulation.step_until_convergence() + logger.debug(f"IK success: {robot.get_state().ik_success}") + logger.debug(f"sim converged: {simulation.is_converged()}") + + if ROBOT_INSTANCE == RobotInstance.HARDWARE: + input( + "hold an object 25cm in front of the gripper, the robot is going to grasp it, press enter when you are ready" + ) + + # move 25cm towards the gripper direction + robot.set_cartesian_position( + robot.get_cartesian_position() * rcsss.common.Pose(translation=np.array([0, 0, 0.25])) + ) + if ROBOT_INSTANCE == RobotInstance.SIMULATION: + simulation.step_until_convergence() + logger.debug(f"IK success: {robot.get_state().ik_success}") + logger.debug(f"sim converged: {simulation.is_converged()}") + + # grasp the object + gripper.grasp() + if ROBOT_INSTANCE == RobotInstance.SIMULATION: + simulation.step_until_convergence() + logger.debug(f"sim converged: {simulation.is_converged()}") + + # move 25cm backward + robot.set_cartesian_position( + robot.get_cartesian_position() * rcsss.common.Pose(translation=np.array([0, 0, -0.25])) + ) + if ROBOT_INSTANCE == RobotInstance.SIMULATION: + simulation.step_until_convergence() + logger.debug(f"IK success: {robot.get_state().ik_success}") + logger.debug(f"sim converged: {simulation.is_converged()}") + + if ROBOT_INSTANCE == RobotInstance.HARDWARE: + input("gripper is going to be open, press enter when you are ready") + + # open gripper + gripper.open() + if ROBOT_INSTANCE == RobotInstance.SIMULATION: + simulation.step_until_convergence() + + # move back to home position + robot.move_home() + if ROBOT_INSTANCE == RobotInstance.SIMULATION: + simulation.step_until_convergence() + + +if __name__ == "__main__": + main() diff --git a/python/rcsss/envs/base.py b/python/rcsss/envs/base.py index c0e453b6..7a089ca3 100644 --- a/python/rcsss/envs/base.py +++ b/python/rcsss/envs/base.py @@ -241,6 +241,7 @@ def reset( if options is not None: msg = "options not implemented yet" raise NotImplementedError(msg) + self.robot.reset() return self.get_obs(), {} @@ -438,12 +439,10 @@ def __init__(self, env, gripper: common.Gripper, binary: bool = True): self.action_space.spaces.update(get_space(GripperDictType).spaces) self.gripper_key = get_space_keys(GripperDictType)[0] self._gripper = gripper - self._last_gripper_action = 1 self.binary = binary def reset(self, **kwargs) -> tuple[dict[str, Any], dict[str, Any]]: self._gripper.reset() - self._last_gripper_action = 1 return super().reset(**kwargs) def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tuple[dict[str, Any], dict[str, Any]]: @@ -464,8 +463,10 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: assert self.gripper_key in action, "Gripper action not found." gripper_action = np.round(action["gripper"]) - if gripper_action != self._last_gripper_action: - self._last_gripper_action = gripper_action + width = self._gripper.get_normalized_width() + if self.binary: + width = np.round(width) + if gripper_action != width: if self.binary: self._gripper.grasp() if gripper_action == 0 else self._gripper.open() else: diff --git a/python/rcsss/envs/hw.py b/python/rcsss/envs/hw.py index 5f7ea35b..5ce0f755 100644 --- a/python/rcsss/envs/hw.py +++ b/python/rcsss/envs/hw.py @@ -28,5 +28,4 @@ def step(self, action: Any) -> tuple[dict[str, Any], SupportsFloat, bool, bool, def reset( self, seed: int | None = None, options: dict[str, Any] | None = None ) -> tuple[dict[str, Any], dict[str, Any]]: - self.hw_robot.move_home() return super().reset(seed=seed, options=options) diff --git a/python/rcsss/envs/sim.py b/python/rcsss/envs/sim.py index ff41338c..35b61505 100644 --- a/python/rcsss/envs/sim.py +++ b/python/rcsss/envs/sim.py @@ -31,9 +31,9 @@ def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, boo def reset( self, seed: int | None = None, options: dict[str, Any] | None = None ) -> tuple[dict[str, Any], dict[str, Any]]: - self.sim_robot.reset() + obs, info = super().reset(seed=seed, options=options) self.sim.step(1) - return super().reset(seed=seed, options=options) + return obs, info class CollisionGuard(gym.Wrapper[dict[str, Any], dict[str, Any], dict[str, Any], dict[str, Any]]): diff --git a/python/rcsss/envs/typed_factory.py b/python/rcsss/envs/typed_factory.py deleted file mode 100644 index dd74bc23..00000000 --- a/python/rcsss/envs/typed_factory.py +++ /dev/null @@ -1,536 +0,0 @@ -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.realsense import RealSenseCameraSet, RealSenseSetConfig -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, - JointsDictType, - LimitedJointsRelDictType, - LimitedTQuartRelDictType, - LimitedTRPYRelDictType, - ObsArmsGr, - ObsArmsGrCam, - RelativeActionSpace, - TQuartDictType, - TRPYDictType, -) -from rcsss.envs.hw import FR3HW -from rcsss.envs.sim import CollisionGuard, FR3Sim - - -def produce_env_sim( - 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) - 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, - } - - -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, - ) - - -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, - ) - - -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 - ) - 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", -) -> 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"] - gripper = sim.FrankaHand(simulation, robot_id, gripper_cfg) - env_sim_gripper = GripperWrapper(env_sim, gripper) - camera_set = SimCameraSet(simulation, cam_cfg) - env_sim_gripper_cam: gym.Env = CameraSetWrapper(env_sim_gripper, camera_set) - return typing.cast( - gym.Env[ObsArmsGrCam, TQuartDictType], - 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: 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) - env_rel = RelativeActionSpace(env_cam) - return typing.cast( - gym.Env[ObsArmsGrCam, LimitedTQuartRelDictType], - env_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[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_cg = CollisionGuard.env_from_xml_paths( - env_sim_tquart_gripper_cam, mjcf_path, urdf_path, gripper=True, check_home_collision=False, camera=True - ) - return typing.cast( - gym.Env[ObsArmsGrCam, TQuartDictType], - 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: 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_cg = RelativeActionSpace(env_sim_tquart_gripper_cam) - return typing.cast( - gym.Env[ObsArmsGrCam, LimitedTQuartRelDictType], - 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: 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) - simulation = info["sim"] - gripper = sim.FrankaHand(simulation, robot_id, gripper_cfg) - env_sim_trpy_gripper = GripperWrapper(env_sim_trpy, gripper) - camera_set = SimCameraSet(simulation, cam_cfg) - env_sim_trpy_gripper_cam: gym.Env = CameraSetWrapper(env_sim_trpy_gripper, camera_set) - return typing.cast( - gym.Env[ObsArmsGrCam, TRPYDictType], - 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: 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_rel = RelativeActionSpace(env_sim_trpy_gripper_cam) - return typing.cast( - gym.Env[ObsArmsGrCam, LimitedTRPYRelDictType], - 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: 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_cg = CollisionGuard.env_from_xml_paths( - env_sim_trpy_gripper_cam, mjcf_path, urdf_path, gripper=True, check_home_collision=False, camera=True - ) - return typing.cast( - gym.Env[ObsArmsGrCam, TRPYDictType], - 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: 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_rel = RelativeActionSpace(env_sim_trpy_gripper_camera_cg) - return typing.cast( - gym.Env[ObsArmsGrCam, LimitedTRPYRelDictType], - 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[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_gripper = GripperWrapper(env_sim_joints, gripper) - camera_set = SimCameraSet(simulation, cam_cfg) - env_sim_joints_gripper_cam: gym.Env = CameraSetWrapper(env_sim_joints_gripper, camera_set) - return typing.cast( - gym.Env[ObsArmsGrCam, JointsDictType], - 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[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_rel = RelativeActionSpace(env_sim_joints_cam) - return typing.cast( - gym.Env[ObsArmsGrCam, LimitedJointsRelDictType], - 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: 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_cg = CollisionGuard.env_from_xml_paths( - env_sim_joints_gripper_cam, mjcf_path, urdf_path, gripper=True, check_home_collision=False, camera=True - ) - return typing.cast( - gym.Env[ObsArmsGrCam, JointsDictType], - 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: 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_rel = RelativeActionSpace(env_sim_joints_gripper_cam_cg) - return typing.cast( - gym.Env[ObsArmsGrCam, LimitedJointsRelDictType], - env_sim_joints_gripper_cam_cg_rel, - ) - - -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(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]: - 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_cg( - ip: str, urdf_path: str, mjcf_path: str, gripper_cfg: FHConfig, cfg_path: str -) -> gym.Env[ObsArmsGr, JointsDictType]: - env_hw_joints_gripper = produce_env_hw_joints_gripper(ip, urdf_path, gripper_cfg, cfg_path) - env_hw_joints_gripper_cg = CollisionGuard.env_from_xml_paths( - env_hw_joints_gripper, mjcf_path, urdf_path, gripper=True, check_home_collision=False, camera=True - ) - return typing.cast( - gym.Env[ObsArmsGr, JointsDictType], - env_hw_joints_gripper_cg, - ) - - -def produce_env_hw_joints_gripper_cg_rel( - ip: str, urdf_path: str, mjcf_path: str, gripper_cfg: FHConfig, cfg_path: str -) -> gym.Env[ObsArmsGr, LimitedJointsRelDictType]: - env_hw_joints_gripper_cg = produce_env_hw_joints_gripper_cg(ip, urdf_path, mjcf_path, gripper_cfg, cfg_path) - env_hw_joints_gripper_cg_rel = RelativeActionSpace(env_hw_joints_gripper_cg) - return typing.cast(gym.Env[ObsArmsGr, LimitedJointsRelDictType], env_hw_joints_gripper_cg_rel) - - -def produce_env_hw_joints_gripper_camera( - ip: str, urdf_path: str, gripper_cfg: FHConfig, cam_cfg: RealSenseSetConfig, cfg_path: str -) -> (gym.Env)[ObsArmsGrCam, JointsDictType]: - 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) - 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) - 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, camera=True - ) - 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_cg = produce_env_hw_joints_gripper_camera_cg( - ip, urdf_path, mjcf_path, gripper_cfg, cam_cfg, cfg_path - ) - env_hw_joints_gripper_cam_cg_rel = RelativeActionSpace(env_hw_joints_gripper_cam_cg) - return typing.cast(gym.Env[ObsArmsGrCam, LimitedJointsRelDictType], env_hw_joints_gripper_cam_cg_rel) - - -def produce_env_hw_trpy(ip: str, urdf_path: str, cfg_path: str) -> gym.Env[ArmObsType, TRPYDictType]: - 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]: - 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]: - 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]: - 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]: - 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, camera=True - ) - 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) - 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, camera=True - ) - 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]: - 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]: - 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]: - 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) - 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) - 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, camera=True - ) - 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 - ) - 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, camera=True - ) - return typing.cast(gym.Env[ObsArmsGrCam, LimitedTQuartRelDictType], env_hw_tquart_gripper_cam_cg) - - -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() diff --git a/python/tests/test_sim_envs.py b/python/tests/test_sim_envs.py deleted file mode 100644 index b9c757ba..00000000 --- a/python/tests/test_sim_envs.py +++ /dev/null @@ -1,360 +0,0 @@ -from collections import OrderedDict - -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 - - -@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") - - 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." - - -class TestSimEnvsTRPY: - """This class is for testing TRPY sim env functionalities""" - - 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, _ = 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)]) - 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 = initial_pose - assert out_pose.is_close(expected_pose, 1e-2, 1e-3) - - 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, _ = env.reset() - # action to be performed` - x_pos_change = 0.2 - initial_tquart = obs_initial["tquart"].copy() - t = initial_tquart[:3] - # shift the translation in x - t[0] += x_pos_change - 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", 0)]) - expected_obs = obs_initial["tquart"].copy() - expected_obs[0] += x_pos_change - 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, 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, _ = env.reset() - - # action to be performed - 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"] - - 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) - - 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, _ = env.reset() - - # 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", 0), - ] - ) - expected_obs = obs_initial["tquart"].copy() - - expected_obs[0] += x_pos_change - 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, 1e-2, 1e-3) - - # 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 - """ - - 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, _ = env_.reset() - # an obvious below ground collision action - obs["xyzrpy"][2] = -0.2 - - collision_action = OrderedDict( - [ - ("xyzrpy", obs["xyzrpy"]), - ("gripper", 0), - ] - ) - _, _, _, _, 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["is_sim_converged"] - 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, _ = 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))]) - - expected_obs = obs_initial["tquart"].copy() - expected_obs[0] += x_pos_change - - 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) - - 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() - - home_action_vec = obs_initial["tquart"] - zero_action = OrderedDict([("tquart", home_action_vec)]) - - 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, 1e-2, 1e-3) - - 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, _ = 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", 0), - ] - ) - 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, 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 - """ - - 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", - ) - 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", 0), - ] - ) - _, _, _, _, info = env_.step(act) - - assert info["ik_success"] - assert info["is_sim_converged"] - 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, _ = env_.reset() - - # action to be performed - 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) - - 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, _ = env_.reset() - # action to be performed - 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): - """ - 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, - gripper_cfg=gripper_cfg, - cam_cfg=cam_cfg, - robot_id="0", - ) - 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", 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): - """ - 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, _ = env_.reset() - act = OrderedDict( - [ - ("joints", np.array([0, 0, 0, 0, 0, 0, 0], dtype=np.float32)), - ("gripper", 0), - ] - ) - 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) diff --git a/src/hw/FR3.cpp b/src/hw/FR3.cpp index 7eb27e2f..013fc578 100644 --- a/src/hw/FR3.cpp +++ b/src/hw/FR3.cpp @@ -130,7 +130,10 @@ void FR3::move_home() { this->set_joint_position(q_home); } void FR3::automatic_error_recovery() { this->robot.automaticErrorRecovery(); } -void FR3::reset() {} +void FR3::reset() { + this->automatic_error_recovery(); + this->move_home(); +} void FR3::wait_milliseconds(int milliseconds) { std::this_thread::sleep_for(std::chrono::milliseconds(milliseconds)); diff --git a/src/hw/FrankaHand.cpp b/src/hw/FrankaHand.cpp index 164432eb..7ec33e85 100644 --- a/src/hw/FrankaHand.cpp +++ b/src/hw/FrankaHand.cpp @@ -13,10 +13,7 @@ namespace hw { FrankaHand::FrankaHand(const std::string &ip, const FHConfig &cfg) : gripper(ip), cfg{} { this->cfg = cfg; - franka::GripperState gripper_state = gripper.readOnce(); - this->max_width = gripper_state.max_width - 0.001; - this->last_commanded_width = this->max_width; - gripper.move(this->max_width, this->cfg.speed); + this->m_reset(); } FrankaHand::~FrankaHand() {} @@ -71,12 +68,15 @@ double FrankaHand::get_normalized_width() { return gripper_state.width / gripper_state.max_width; } -void FrankaHand::reset() { +void FrankaHand::m_reset() { this->gripper.stop(); + // open gripper franka::GripperState gripper_state = gripper.readOnce(); this->max_width = gripper_state.max_width - 0.001; - this->open(); + this->last_commanded_width = this->max_width; + gripper.move(this->max_width, this->cfg.speed); } +void FrankaHand::reset() { this->m_reset(); } bool FrankaHand::is_grasped() { franka::GripperState gripper_state = gripper.readOnce(); diff --git a/src/hw/FrankaHand.h b/src/hw/FrankaHand.h index 340f305f..90bfc448 100644 --- a/src/hw/FrankaHand.h +++ b/src/hw/FrankaHand.h @@ -40,6 +40,7 @@ class FrankaHand : public common::Gripper { FHConfig cfg; double max_width; double last_commanded_width; + void m_reset(); public: FrankaHand(const std::string &ip, const FHConfig &cfg); diff --git a/src/sim/FR3.cpp b/src/sim/FR3.cpp index 4062105f..d0d6280e 100644 --- a/src/sim/FR3.cpp +++ b/src/sim/FR3.cpp @@ -145,8 +145,8 @@ FR3State* FR3::get_state() { } common::Pose FR3::get_cartesian_position() { - Eigen::Matrix rotation(this->sim->d->site_xmat + - 9 * this->ids.attachment_site); + Eigen::Matrix rotation( + this->sim->d->site_xmat + 9 * this->ids.attachment_site); Eigen::Vector3d translation(this->sim->d->site_xpos + 3 * this->ids.attachment_site); common::Pose attachment_site(Eigen::Matrix3d(rotation), translation); @@ -181,6 +181,8 @@ void FR3::set_cartesian_position(const common::Pose& pose) { this->state.ik_success = true; this->rl.kin->forwardPosition(); this->set_joint_position(this->rl.kin->getPosition()); + // forward kinematics can be accessed by + // this->rl.kin->getOperationalPosition(0); } else { this->state.ik_success = false; } diff --git a/src/sim/FrankaHand.cpp b/src/sim/FrankaHand.cpp index 26c25de4..6abd839b 100644 --- a/src/sim/FrankaHand.cpp +++ b/src/sim/FrankaHand.cpp @@ -15,7 +15,8 @@ struct { std::vector collision_geoms_fingers{"finger_0_left", "finger_0_right"}; - std::string joint = "finger_joint1"; + std::string joint1 = "finger_joint1"; + std::string joint2 = "finger_joint2"; std::string actuator = "actuator8"; } const gripper_names; @@ -34,11 +35,17 @@ FrankaHand::FrankaHand(std::shared_ptr sim, const std::string &id, } // this->tendon_id = // mj_name2id(this->sim->m, mjOBJ_TENDON, ("split_" + id).c_str()); - this->joint_id = this->sim->m->jnt_qposadr[mj_name2id( - this->sim->m, mjOBJ_JOINT, (gripper_names.joint + "_" + id).c_str())]; - if (this->joint_id == -1) { + this->joint_id_1 = this->sim->m->jnt_qposadr[mj_name2id( + this->sim->m, mjOBJ_JOINT, (gripper_names.joint1 + "_" + id).c_str())]; + if (this->joint_id_1 == -1) { throw std::runtime_error( - std::string("No joint named " + gripper_names.joint)); + std::string("No joint named " + gripper_names.joint1)); + } + this->joint_id_2 = this->sim->m->jnt_qposadr[mj_name2id( + this->sim->m, mjOBJ_JOINT, (gripper_names.joint2 + "_" + id).c_str())]; + if (this->joint_id_2 == -1) { + throw std::runtime_error( + std::string("No joint named " + gripper_names.joint2)); } // Collision geoms this->add_collision_geoms(gripper_names.collision_geoms, this->cgeom, false); @@ -104,7 +111,9 @@ void FrankaHand::set_normalized_width(double width, double force) { // this->sim->d->actuator_force[this->gripper_id] = 0; } double FrankaHand::get_normalized_width() { - double width = this->sim->d->qpos[this->joint_id] / this->MAX_JOINT_WIDTH; + // TODO: maybe we should use the mujoco sensors? Not sure what the difference + // is between reading out from qpos and reading from the sensors. + double width = this->sim->d->qpos[this->joint_id_1] / this->MAX_JOINT_WIDTH; // sometimes the joint is slightly outside of the bounds if (width < 0) { width = 0; @@ -167,9 +176,11 @@ void FrankaHand::m_reset() { this->state = FHState(); this->state.max_unnormalized_width = this->MAX_WIDTH; // reset state hard - this->sim->d->qpos[this->joint_id] = this->MAX_JOINT_WIDTH; + this->sim->d->qpos[this->joint_id_1] = this->MAX_JOINT_WIDTH; + this->sim->d->qpos[this->joint_id_2] = this->MAX_JOINT_WIDTH; + this->sim->d->ctrl[this->actuator_id] = this->MAX_WIDTH; } void FrankaHand::reset() { this->m_reset(); } } // namespace sim -} // namespace rcs \ No newline at end of file +} // namespace rcs diff --git a/src/sim/FrankaHand.h b/src/sim/FrankaHand.h index 7649279b..1638421e 100644 --- a/src/sim/FrankaHand.h +++ b/src/sim/FrankaHand.h @@ -35,7 +35,8 @@ class FrankaHand : public common::Gripper { FHConfig cfg; std::shared_ptr sim; int actuator_id; - int joint_id; + int joint_id_1; + int joint_id_2; FHState state; std::string id; void is_moving_callback();