diff --git a/README.md b/README.md index 69eabdd6..e5bf2b2b 100644 --- a/README.md +++ b/README.md @@ -90,7 +90,7 @@ if __name__ == "__main__": for _ in range(10): # move 1cm in x direction (forward) and close gripper - act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 0} + act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "gripper": [0]} obs, reward, terminated, truncated, info = env.step(act) print(obs) ``` diff --git a/examples/fr3/fr3_env_cartesian_control.py b/examples/fr3/fr3_env_cartesian_control.py index a34104bf..e62bd0c0 100644 --- a/examples/fr3/fr3_env_cartesian_control.py +++ b/examples/fr3/fr3_env_cartesian_control.py @@ -61,11 +61,11 @@ def main(): for _ in range(100): for _ in range(10): # move 1cm in x direction (forward) and close gripper - act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 0} + act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "gripper": [0]} obs, reward, terminated, truncated, info = env_rel.step(act) for _ in range(10): # move 1cm in negative x direction (backward) and open gripper - act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": 1} + act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": [1]} obs, reward, terminated, truncated, info = env_rel.step(act) diff --git a/examples/fr3/fr3_readme.py b/examples/fr3/fr3_readme.py index 03f10a50..3f27c72f 100644 --- a/examples/fr3/fr3_readme.py +++ b/examples/fr3/fr3_readme.py @@ -67,6 +67,6 @@ for _ in range(10): # move 1cm in x direction (forward) and close gripper - act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 0} + act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "gripper": [0]} obs, reward, terminated, truncated, info = env.step(act) print(obs) diff --git a/examples/fr3/grasp_demo.py b/examples/fr3/grasp_demo.py index 606c8d63..7fb5564f 100644 --- a/examples/fr3/grasp_demo.py +++ b/examples/fr3/grasp_demo.py @@ -19,8 +19,8 @@ def __init__(self, env: gym.Env): self.unwrapped: RobotEnv = cast(RobotEnv, self.env.unwrapped) self.home_pose = self.unwrapped.robot.get_cartesian_position() - def _action(self, pose: Pose, gripper: float) -> dict[str, Any]: - return {"xyzrpy": pose.xyzrpy(), "gripper": gripper} + def _action(self, pose: Pose, gripper: list[float]) -> dict[str, Any]: + return {"xyzrpy": pose.xyzrpy(), "gripper": [gripper]} def get_object_pose(self, geom_name) -> Pose: model = self.env.get_wrapper_attr("sim").model @@ -50,7 +50,7 @@ def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int goal_pose *= Pose(translation=np.array([0, 0, delta_up]), quaternion=np.array([1, 0, 0, 0])) # type: ignore return self.generate_waypoints(end_eff_pose, goal_pose, num_waypoints=num_waypoints) - def execute_motion(self, waypoints: list[Pose], gripper: float = GripperWrapper.BINARY_GRIPPER_OPEN) -> dict: + def execute_motion(self, waypoints: list[Pose], gripper: list[float] = GripperWrapper.BINARY_GRIPPER_OPEN) -> dict: for i in range(len(waypoints)): obs = self.step(self._action(waypoints[i], gripper)) return obs diff --git a/examples/fr3/grasp_digit_demo.py b/examples/fr3/grasp_digit_demo.py index 6ba56115..1f746b66 100644 --- a/examples/fr3/grasp_digit_demo.py +++ b/examples/fr3/grasp_digit_demo.py @@ -19,7 +19,7 @@ def __init__(self, env: gym.Env): self.unwrapped: RobotEnv = cast(RobotEnv, self.env.unwrapped) self.home_pose = self.unwrapped.robot.get_cartesian_position() - def _action(self, pose: Pose, gripper: float) -> dict[str, Any]: + def _action(self, pose: Pose, gripper: list[float]) -> dict[str, Any]: return {"xyzrpy": pose.xyzrpy(), "gripper": gripper} def get_object_pose(self, geom_name) -> Pose: @@ -48,7 +48,7 @@ def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int goal_pose *= Pose(translation=np.array([0, 0, delta_up]), quaternion=np.array([1, 0, 0, 0])) # type: ignore return self.generate_waypoints(end_eff_pose, goal_pose, num_waypoints=num_waypoints) - def execute_motion(self, waypoints: list[Pose], gripper: float = GripperWrapper.BINARY_GRIPPER_OPEN) -> dict: + def execute_motion(self, waypoints: list[Pose], gripper: list[float] = GripperWrapper.BINARY_GRIPPER_OPEN) -> dict: for i in range(len(waypoints)): obs = self.step(self._action(waypoints[i], gripper)) return obs diff --git a/examples/fr3/grasp_ompl_demo.py b/examples/fr3/grasp_ompl_demo.py index e8cda012..0a05fd5d 100644 --- a/examples/fr3/grasp_ompl_demo.py +++ b/examples/fr3/grasp_ompl_demo.py @@ -45,9 +45,9 @@ def __init__(self, env: gym.Env, planner: MjOMPL): self.planner = planner def _action(self, pose: Pose, gripper: float) -> dict[str, Any]: - return {"xyzrpy": pose.xyzrpy(), "gripper": gripper} + return {"xyzrpy": pose.xyzrpy(), "gripper": [gripper]} - def _jaction(self, joints: np.ndarray, gripper: float) -> dict[str, Any]: + def _jaction(self, joints: np.ndarray, gripper: list[float]) -> dict[str, Any]: return {"joints": joints, "gripper": gripper} def get_object_pose(self, geom_name) -> Pose: @@ -102,7 +102,7 @@ def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: in def step(self, action: dict) -> dict: return self.env.step(action)[0] - def execute_motion(self, waypoints: list[Pose], gripper: float = GripperWrapper.BINARY_GRIPPER_OPEN) -> dict: + def execute_motion(self, waypoints: list[Pose], gripper: list[float] = GripperWrapper.BINARY_GRIPPER_OPEN) -> dict: for i in range(len(waypoints)): obs = self.step(self._jaction(waypoints[i], gripper)) # type: ignore return obs diff --git a/examples/so101/so101_env_cartesian_control.py b/examples/so101/so101_env_cartesian_control.py index f77933f8..19c9e4e7 100644 --- a/examples/so101/so101_env_cartesian_control.py +++ b/examples/so101/so101_env_cartesian_control.py @@ -49,13 +49,13 @@ def main(): env_rel.get_wrapper_attr("sim").open_gui() obs, info = env_rel.reset() - act = {"tquat": [0.03, 0, 0, 0, 0, 0, 1], "gripper": 1} + act = {"tquat": [0.03, 0, 0, 0, 0, 0, 1], "gripper": [1]} obs, reward, terminated, truncated, info = env_rel.step(act) for _ in range(100): for _ in range(5): # move 1cm in x direction (forward) and close gripper - act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": 0} + act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": [0]} obs, reward, terminated, truncated, info = env_rel.step(act) print(info, obs) if truncated or terminated: @@ -63,7 +63,7 @@ def main(): return for _ in range(5): # move 1cm in negative x direction (backward) and open gripper - act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 1} + act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "gripper": [1]} obs, reward, terminated, truncated, info = env_rel.step(act) if truncated or terminated: logger.info("Truncated or terminated!") diff --git a/examples/ur5e/ur5e_env_cartesian_control.py b/examples/ur5e/ur5e_env_cartesian_control.py index bf46e8da..f0a3eefb 100644 --- a/examples/ur5e/ur5e_env_cartesian_control.py +++ b/examples/ur5e/ur5e_env_cartesian_control.py @@ -54,6 +54,10 @@ def main(): gripper_config.joints = ["right_driver_joint"] gripper_config.collision_geoms = [] gripper_config.collision_geoms_fingers = [] + gripper_config.max_actuator_width = 0 + gripper_config.min_actuator_width = 1 + gripper_config.max_joint_width = 0.0 + gripper_config.min_joint_width = 0.8 env_rel = SimEnvCreator()( control_mode=ControlMode.CARTESIAN_TQuat, @@ -70,12 +74,12 @@ def main(): for _ in range(100): for _ in range(10): # move 1cm in x direction (forward) and close gripper - act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1.0], "gripper": 0} + act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1.0], "gripper": [0]} obs, reward, terminated, truncated, info = env_rel.step(act) sleep(0.6) for _ in range(10): # move 1cm in negative x direction (backward) and open gripper - act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1.0], "gripper": 1} + act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1.0], "gripper": [1]} obs, reward, terminated, truncated, info = env_rel.step(act) sleep(0.6) diff --git a/examples/ur5e/ur5e_env_joint_control.py b/examples/ur5e/ur5e_env_joint_control.py index 139b9c39..5177d380 100644 --- a/examples/ur5e/ur5e_env_joint_control.py +++ b/examples/ur5e/ur5e_env_joint_control.py @@ -54,6 +54,10 @@ def main(): gripper_config.joints = ["right_driver_joint"] gripper_config.collision_geoms = [] gripper_config.collision_geoms_fingers = [] + gripper_config.max_actuator_width = 0 + gripper_config.min_actuator_width = 1 + gripper_config.max_joint_width = 0.0 + gripper_config.min_joint_width = 0.8 env_rel = SimEnvCreator()( control_mode=ControlMode.JOINTS, diff --git a/extensions/rcs_panda/src/rcs_panda/panda_env_cartesian_control.py b/extensions/rcs_panda/src/rcs_panda/panda_env_cartesian_control.py index f15779d2..418bbf2f 100644 --- a/extensions/rcs_panda/src/rcs_panda/panda_env_cartesian_control.py +++ b/extensions/rcs_panda/src/rcs_panda/panda_env_cartesian_control.py @@ -29,14 +29,14 @@ def main(): for _ in range(100): for _ in range(10): # move 1cm in x direction (forward) and close gripper - act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 0} + act = {"tquat": [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 = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": 1} + act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": [1]} obs, reward, terminated, truncated, info = env_rel.step(act) if truncated or terminated: logger.info("Truncated or terminated!") diff --git a/extensions/rcs_tacto/pyproject.toml b/extensions/rcs_tacto/pyproject.toml index d1ffc97f..96370b42 100644 --- a/extensions/rcs_tacto/pyproject.toml +++ b/extensions/rcs_tacto/pyproject.toml @@ -4,10 +4,11 @@ build-backend = "setuptools.build_meta" [project] name = "rcs_tacto" -version = "0.5.0" +version = "0.6.0" description="RCS integration of tacto" dependencies = [ - "rcs>=0.5.2", + "rcs>=0.6.0", + "omegaconf", "mujoco-tacto@git+https://github.com/utn-air/mujoco-tacto.git@main" ] readme = "README.md" diff --git a/extensions/rcs_tacto/src/rcs_tacto/__init__.py b/extensions/rcs_tacto/src/rcs_tacto/__init__.py index 72251527..906d362f 100644 --- a/extensions/rcs_tacto/src/rcs_tacto/__init__.py +++ b/extensions/rcs_tacto/src/rcs_tacto/__init__.py @@ -1 +1 @@ -__version__ = "0.5.2" +__version__ = "0.6.0" diff --git a/extensions/rcs_tacto/src/rcs_tacto/tacto_wrapper.py b/extensions/rcs_tacto/src/rcs_tacto/tacto_wrapper.py index 36d65dbe..89e30956 100644 --- a/extensions/rcs_tacto/src/rcs_tacto/tacto_wrapper.py +++ b/extensions/rcs_tacto/src/rcs_tacto/tacto_wrapper.py @@ -5,8 +5,8 @@ import cv2 import gymnasium as gym -import hydra import tacto +from omegaconf import OmegaConf logger = logging.getLogger(__name__) @@ -51,8 +51,8 @@ def __init__( if tacto_bg is None: tacto_bg = str(files("tacto") / "assets" / "bg_digit_240_320.jpg") logger.warning(f"No tacto_bg provided, using default from package: {tacto_bg}") - hydra.initialize_config_dir(tacto_config) - t_config = hydra.compose("digit.yaml") + config_path = os.path.join(tacto_config, "digit.yaml") + t_config = OmegaConf.load(config_path) self.tacto_sensor = tacto.Sensor(**t_config.tacto, background=cv2.imread(tacto_bg)) self.tacto_fps = tacto_fps self.tacto_last_render = -1 diff --git a/pyproject.toml b/pyproject.toml index 864318a5..f89a8f19 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -192,4 +192,8 @@ version_files = [ "extensions/rcs_ur5e/pyproject.toml:version", "extensions/rcs_ur5e/src/rcs_ur5e/__init__.py:__version__", "extensions/rcs_ur5e/pyproject.toml:\"rcs>=(.*)\"", + + "extensions/rcs_tacto/pyproject.toml:version", + "extensions/rcs_tacto/src/rcs_tacto/__init__.py:__version__", + "extensions/rcs_tacto/pyproject.toml:\"rcs>=(.*)\"", ] diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index 8a6dbdee..1518909d 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -3,7 +3,7 @@ import copy import logging from enum import Enum, auto -from typing import Annotated, Any, Literal, TypeAlias, cast +from typing import Annotated, Any, ClassVar, Literal, TypeAlias, cast import gymnasium as gym import numpy as np @@ -684,8 +684,8 @@ def close(self): class GripperWrapper(ActObsInfoWrapper): # TODO: sticky gripper, like in aloha - BINARY_GRIPPER_CLOSED = 0 - BINARY_GRIPPER_OPEN = 1 + BINARY_GRIPPER_CLOSED: ClassVar[list[float]] = [0] + BINARY_GRIPPER_OPEN: ClassVar[list[float]] = [1] def __init__(self, env, gripper: common.Gripper, binary: bool = True): super().__init__(env) @@ -715,7 +715,7 @@ def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tupl self._last_gripper_cmd if self._last_gripper_cmd is not None else self.BINARY_GRIPPER_OPEN ) else: - observation[self.gripper_key] = self.gripper.get_normalized_width() + observation[self.gripper_key] = [self.gripper.get_normalized_width()] return observation, info @@ -724,13 +724,17 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: action = copy.deepcopy(action) assert self.gripper_key in action, "Gripper action not found." - gripper_action = np.round(action[self.gripper_key]) if self.binary else action[self.gripper_key] + gripper_action = action[self.gripper_key] + if isinstance(gripper_action, int | float): + gripper_action = [gripper_action] # type: ignore + if self.binary: + gripper_action = np.round(gripper_action) gripper_action = np.clip(gripper_action, 0.0, 1.0) if self.binary: self.gripper.grasp() if gripper_action == self.BINARY_GRIPPER_CLOSED else self.gripper.open() else: - self.gripper.set_normalized_width(gripper_action) + self.gripper.set_normalized_width(next(gripper_action)) self._last_gripper_cmd = gripper_action del action[self.gripper_key] return action