diff --git a/assets/objects/green_cube/green_cube.xml b/assets/objects/green_cube/green_cube.xml index 55f715ee..1becc3da 100644 --- a/assets/objects/green_cube/green_cube.xml +++ b/assets/objects/green_cube/green_cube.xml @@ -6,7 +6,7 @@ - + diff --git a/assets/objects/green_cuboid/green_cuboid.xml b/assets/objects/green_cuboid/green_cuboid.xml new file mode 100644 index 00000000..550bda14 --- /dev/null +++ b/assets/objects/green_cuboid/green_cuboid.xml @@ -0,0 +1,14 @@ + + + + diff --git a/assets/objects/red_cube/red_cube.xml b/assets/objects/red_cube/red_cube.xml index e094e847..4b29fcfb 100644 --- a/assets/objects/red_cube/red_cube.xml +++ b/assets/objects/red_cube/red_cube.xml @@ -6,7 +6,7 @@ - + diff --git a/examples/fr3/grasp_demo.py b/examples/fr3/grasp_demo.py index 2a7f4ef3..7d29c26b 100644 --- a/examples/fr3/grasp_demo.py +++ b/examples/fr3/grasp_demo.py @@ -92,8 +92,8 @@ def main(): cfg.sim_cfg.async_control = True cfg.max_relative_movement = None cfg.root_frame_objects = { - "green_cube": ( - rcs.OBJECT_PATHS["green_cube"], + "green_cuboid": ( + rcs.OBJECT_PATHS["green_cuboid"], Pose(translation=np.array([0.5, 0.0, 0.05]), quaternion=np.array([0.0, 0.0, 0.0, 1.0])), ) } diff --git a/examples/fr3/grasp_ompl_demo.py b/examples/fr3/grasp_ompl_demo.py index 4641ecd2..0a8866cf 100644 --- a/examples/fr3/grasp_ompl_demo.py +++ b/examples/fr3/grasp_ompl_demo.py @@ -120,8 +120,8 @@ def main(): cfg.sim_cfg.async_control = True cfg.max_relative_movement = None cfg.root_frame_objects = { - "green_cube": ( - rcs.OBJECT_PATHS["green_cube"], + "green_cuboid": ( + rcs.OBJECT_PATHS["green_cuboid"], Pose(translation=np.array([0.5, 0.0, 0.05]), quaternion=np.array([0.0, 0.0, 0.0, 1.0])), ) } diff --git a/extensions/rcs_taxim/src/rcs_taxim/creators.py b/extensions/rcs_taxim/src/rcs_taxim/creators.py index 5fdf2e62..0c3e4984 100644 --- a/extensions/rcs_taxim/src/rcs_taxim/creators.py +++ b/extensions/rcs_taxim/src/rcs_taxim/creators.py @@ -102,7 +102,7 @@ def __call__( cfg.gripper_offsets = {"right": rcs.GRIPPER_MOUNT_OFFSETS[rcs.common.GripperType("Robotiq2F85")]} cfg.root_frame_objects = { "": ( - rcs.OBJECT_PATHS["green_cube"], + rcs.OBJECT_PATHS["green_cuboid"], Pose(translation=np.array([0.5, 0.0, 0.05]), quaternion=np.array([0.0, 0.0, 0.0, 1.0])), ) } diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py index aec119e8..cf7fe01c 100644 --- a/python/rcs/__init__.py +++ b/python/rcs/__init__.py @@ -226,6 +226,7 @@ class RobotMetaConfig: "fr3_single_mount": "assets/objects/fr3_single_mount/fr3_single_mount.xml", "robotiq_d405_mount": "assets/objects/robotiq_d405_mount/robotiq_d405_mount.xml", "droid_wrist_mount": "assets/objects/droid_wrist_mount/droid_wrist_mount.xml", + "green_cuboid": "assets/objects/green_cuboid/green_cuboid.xml", "green_cube": "assets/objects/green_cube/green_cube.xml", "red_cube": "assets/objects/red_cube/red_cube.xml", } diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index 62bc897f..feaf32a6 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -1042,10 +1042,11 @@ def close(self): class GripperWrapper(ActObsInfoWrapper): # TODO: sticky gripper, like in aloha + GRIPPER_THRESHOLD = 0.5 BINARY_GRIPPER_CLOSED: ClassVar[list[float]] = [0] BINARY_GRIPPER_OPEN: ClassVar[list[float]] = [1] - def __init__(self, env, gripper: common.Gripper, binary: bool = True): + def __init__(self, env, gripper: common.Gripper, binary: bool = True, prev_action_obs: bool = False): super().__init__(env) self.binary = binary self.observation_space: gym.spaces.Dict @@ -1055,6 +1056,7 @@ def __init__(self, env, gripper: common.Gripper, binary: bool = True): self.gripper_key = get_space_keys(GripperDictType)[0] self.gripper = gripper self._last_gripper_cmd = None + self.prev_action_obs = prev_action_obs def _command_changed(self, gripper_action: np.ndarray) -> bool: if self._last_gripper_cmd is None: @@ -1098,8 +1100,8 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: gripper_action = np.clip(np.asarray(gripper_action, dtype=np.float32), 0.0, 1.0) if self._command_changed(gripper_action): - if self.binary: - self.gripper.grasp() if gripper_action[0] < 0.5 else self.gripper.open() + if self.prev_action_obs: + self.gripper.grasp() if gripper_action[0] < self.GRIPPER_THRESHOLD else self.gripper.open() else: self.gripper.set_normalized_width(float(gripper_action[0])) self._last_gripper_cmd = gripper_action.tolist() diff --git a/python/rcs/envs/configs.py b/python/rcs/envs/configs.py index 651c9f1a..da792e64 100644 --- a/python/rcs/envs/configs.py +++ b/python/rcs/envs/configs.py @@ -392,7 +392,6 @@ def config(self) -> SimEnvCreatorConfig: world_frame_objects: dict[str, tuple[str, rcs.common.Pose]] | None = None root_frame_objects: dict[str, tuple[str, rcs.common.Pose]] | None = { "duo_mount": (OBJECT_PATHS["fr3_duo_mount"], DEFAULT_TRANSFORMS["FR3_DUOMOUNT_BASE"]), - # "green_cube": (OBJECT_PATHS["green_cube"], Pose(translation=[0.5, 0, 0.5], quaternion=[0, 0, 0, 1])), } robot_frame_objects: dict[str, dict[str, tuple[str, rcs.common.Pose]]] | None = { "left": { diff --git a/python/rcs/envs/storage_wrapper.py b/python/rcs/envs/storage_wrapper.py index 59285f4b..3fe9090d 100644 --- a/python/rcs/envs/storage_wrapper.py +++ b/python/rcs/envs/storage_wrapper.py @@ -329,7 +329,7 @@ def reset(self, *, seed: int | None = None, options: dict[str, Any] | None = Non self._success = False self._prev_action = None self._prev_absolute_action = None - obs, info = self.env.reset() + obs, info = self.env.reset(seed=seed, options=options) self.step_cnt = 0 self.uuid = uuid4() return obs, info diff --git a/python/rcs/envs/tasks.py b/python/rcs/envs/tasks.py index db44ca8b..371dce60 100644 --- a/python/rcs/envs/tasks.py +++ b/python/rcs/envs/tasks.py @@ -148,7 +148,7 @@ class PickTaskConfig(BaseTaskConfig): translation=np.array([0.5, 0.0, 0.05]), quaternion=np.array([0, 0, 0, 1]) ) ) - object_xml = rcs.OBJECT_PATHS["green_cube"] + object_xml = rcs.OBJECT_PATHS["green_cuboid"] object_joint: str = "box_joint" prefix: str = "PickTask_" include_rotation: bool = True