Skip to content

feat(gripper): adds is_grapsed and width to info dict #190

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
juelg merged 2 commits into from
Jun 16, 2025
Merged

feat(gripper): adds is_grapsed and width to info dict #190

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
222d8b3
feat(gripper): adds is_grapsed and width to info dict
juelg Jun 6, 2025
29dad2e
refactor: removed lab specific wrappers
juelg Jun 6, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
67 changes: 0 additions & 67 deletions python/rcs/envs/creators.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,15 +23,13 @@
)
from rcs.envs.hw import FR3HW
from rcs.envs.sim import (
CamRobot,
CollisionGuard,
GripperWrapperSim,
PickCubeSuccessWrapper,
RandomCubePos,
RobotSimWrapper,
SimWrapper,
)
from rcs.envs.space_utils import VecType
from rcs.envs.utils import (
default_fr3_hw_gripper_cfg,
default_fr3_hw_robot_cfg,
Expand Down Expand Up @@ -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")
40 changes: 8 additions & 32 deletions python/rcs/envs/sim.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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__)
Expand Down Expand Up @@ -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


Expand Down Expand Up @@ -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

Expand All @@ -280,17 +282,17 @@ 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
if success:
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
Expand All @@ -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