diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index e2329755..e1c984f4 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -23,7 +23,6 @@ ) from rcs.envs.hw import FR3HW from rcs.envs.sim import ( - CamRobot, CollisionGuard, GripperWrapperSim, PickCubeSuccessWrapper, @@ -31,7 +30,6 @@ RobotSimWrapper, SimWrapper, ) -from rcs.envs.space_utils import VecType from rcs.envs.utils import ( default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg, @@ -352,68 +350,3 @@ def __call__( # type: ignore } return SimTaskEnvCreator()("fr3_simple_pick_up", render_mode, control_mode, delta_actions, cameras) - - -class FR3SimplePickUpSimDigitHandEnvCreator(EnvCreator): - def __call__( # type: ignore - self, - render_mode: str = "human", - control_mode: ControlMode = ControlMode.CARTESIAN_TRPY, - resolution: tuple[int, int] | None = None, - frame_rate: int = 0, - delta_actions: bool = True, - ) -> gym.Env: - if resolution is None: - resolution = (256, 256) - - cameras = { - "wrist": SimCameraConfig( - identifier="wrist_0", - type=CameraType.fixed, - resolution_height=resolution[1], - resolution_width=resolution[0], - frame_rate=frame_rate, - ) - } - - return SimTaskEnvCreator()("fr3_simple_pick_up_digit_hand", render_mode, control_mode, delta_actions, cameras) - - -class FR3LabPickUpSimDigitHandEnvCreator(EnvCreator): - def __call__( # type: ignore - self, - cam_robot_joints: VecType, - render_mode: str = "human", - control_mode: ControlMode = ControlMode.CARTESIAN_TRPY, - resolution: tuple[int, int] | None = None, - frame_rate: int = 0, - delta_actions: bool = True, - ) -> gym.Env: - if resolution is None: - resolution = (256, 256) - - cameras = { - "wrist": SimCameraConfig( - identifier="wrist_0", - type=CameraType.fixed, - resolution_height=resolution[1], - resolution_width=resolution[0], - frame_rate=frame_rate, - ), - "side": SimCameraConfig( - identifier="wrist_1", - type=CameraType.fixed, - resolution_height=resolution[1], - resolution_width=resolution[0], - frame_rate=frame_rate, - ), - } - - env_rel = SimTaskEnvCreator()( - "lab_simple_pick_up_digit_hand", - render_mode, - control_mode, - delta_actions, - cameras, - ) - return CamRobot(env_rel, cam_robot_joints, "lab_simple_pick_up_digit_hand") diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index 84413506..c1698e1c 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -6,7 +6,7 @@ import rcs from rcs import sim from rcs.envs.base import ControlMode, GripperWrapper, MultiRobotWrapper, RobotEnv -from rcs.envs.space_utils import ActObsInfoWrapper, VecType +from rcs.envs.space_utils import ActObsInfoWrapper from rcs.envs.utils import default_fr3_sim_robot_cfg logger = logging.getLogger(__name__) @@ -108,6 +108,8 @@ def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tupl state = self._gripper.get_state() if "collision" not in info or not info["collision"]: info["collision"] = state.collision + info["gripper_width"] = self._gripper.get_normalized_width() + info["is_grasped"] = self._gripper.get_normalized_width() > 0.01 and self._gripper.get_normalized_width() < 0.99 return observation, info @@ -258,9 +260,9 @@ def reset( pos_y = iso_cube[1] + np.random.random() * 0.2 - 0.1 if self.include_rotation: - self.sim.data.joint("box-joint").qpos = [pos_x, pos_y, pos_z, 2 * np.random.random() - 1, 0, 0, 1] + self.sim.data.joint("box_joint").qpos = [pos_x, pos_y, pos_z, 2 * np.random.random() - 1, 0, 0, 1] else: - self.sim.data.joint("box-joint").qpos = [pos_x, pos_y, pos_z, 0, 0, 0, 1] + self.sim.data.joint("box_joint").qpos = [pos_x, pos_y, pos_z, 0, 0, 0, 1] return obs, info @@ -280,7 +282,7 @@ def step(self, action: dict[str, Any]): obs, reward, _, truncated, info = super().step(action) success = ( - self.sim.data.joint("box-joint").qpos[2] > 0.15 + 0.852 + self.sim.data.joint("box_joint").qpos[2] > 0.15 + 0.852 and obs["gripper"] == GripperWrapper.BINARY_GRIPPER_CLOSED ) info["success"] = success @@ -288,9 +290,9 @@ def step(self, action: dict[str, Any]): reward = 5 else: tcp_to_obj_dist = np.linalg.norm( - self.sim.data.joint("box-joint").qpos[:3] - self.unwrapped.robot.get_cartesian_position().translation() + self.sim.data.joint("box_joint").qpos[:3] - self.unwrapped.robot.get_cartesian_position().translation() ) - obj_to_goal_dist = np.linalg.norm(self.sim.data.joint("box-joint").qpos[:3] - self.EE_HOME) + obj_to_goal_dist = np.linalg.norm(self.sim.data.joint("box_joint").qpos[:3] - self.EE_HOME) # old reward # reward = -obj_to_goal_dist - tcp_to_obj_dist @@ -311,29 +313,3 @@ def step(self, action: dict[str, Any]): # normalize reward /= 5 # type: ignore return obs, reward, success, truncated, info - - -class CamRobot(gym.Wrapper): - """Use this wrapper in lab setups where the second arm is used as a camera holder.""" - - def __init__(self, env, cam_robot_joints: VecType, scene: str = "lab_simple_pick_up_digit_hand"): - super().__init__(env) - self.unwrapped: RobotEnv - assert isinstance(self.unwrapped.robot, sim.SimRobot), "Robot must be a sim.SimRobot instance." - self.sim = env.get_wrapper_attr("sim") - cfg = default_fr3_sim_robot_cfg(scene, "1") - self.cam_robot = rcs.sim.SimRobot( - self.sim, env.unwrapped.robot.get_ik(), cfg, register_convergence_callback=False - ) - self.cam_robot.set_parameters(default_fr3_sim_robot_cfg(scene)) - self.cam_robot_joints = cam_robot_joints - - def step(self, action: dict): - self.cam_robot.set_joints_hard(self.cam_robot_joints) - obs, reward, done, truncated, info = super().step(action) - return obs, reward, done, truncated, info - - def reset(self, *, seed=None, options=None): - re = super().reset(seed=seed, options=options) - self.cam_robot.reset() - return re