Skip to content

fix(sim): penetration depth on collision #336

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 5 commits into from
Aug 29, 2026
Merged

fix(sim): penetration depth on collision #336

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
4d3745c
fix(sim): added solref and solimp for cubes
juelg Aug 29, 2026
e30df40
chore(sim): rename cube to cuboid
juelg Aug 29, 2026
d8290b6
chore(sim): add green cuboid
juelg Aug 29, 2026
3b6b4b0
fix: reset in storage wrapper to pass seed and options
juelg Aug 29, 2026
4a9bb13
feat(wrappers): gripper prev action obs and threshold
juelg Aug 29, 2026
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
2 changes: 1 addition & 1 deletion assets/objects/green_cube/green_cube.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@

<worldbody>
<body name="box_body">
<geom type="box" size="0.032 0.016 0.0288" rgba="0 0.984 0.373 1" name="box_geom" friction="1 0.3 0.1" density="50" group="1" />
<geom type="box" size="0.02 0.02 0.02" rgba="0 1 0 1" name="box_geom" friction="0.63 0.3 0.1" priority="1" density="440" group="1" solref="0.004 1" solimp="0.95 0.99 0.001" />
<joint type="free" name="box_joint" />
</body>
</worldbody>
Expand Down
14 changes: 14 additions & 0 deletions assets/objects/green_cuboid/green_cuboid.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,14 @@
<mujoco model="green_cuboid">
<compiler angle="radian" meshdir="assets" autolimits="true"/>

<option cone="elliptic" impratio="10"/>


<worldbody>
<body name="box_body">
<geom type="box" size="0.032 0.016 0.0288" rgba="0 0.984 0.373 1" name="box_geom" friction="1 0.3 0.1" density="50" group="1" priority="1" solref="0.004 1" solimp="0.95 0.99 0.001" />
<joint type="free" name="box_joint" />
</body>
</worldbody>

</mujoco>
2 changes: 1 addition & 1 deletion assets/objects/red_cube/red_cube.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@

<worldbody>
<body name="box_body">
<geom type="box" size="0.02 0.02 0.02" rgba="1 0 0 1" name="box_geom" friction="0.63 0.3 0.1" priority="1" density="440" group="1" />
<geom type="box" size="0.02 0.02 0.02" rgba="1 0 0 1" name="box_geom" friction="0.63 0.3 0.1" priority="1" density="440" group="1" solref="0.004 1" solimp="0.95 0.99 0.001" />
<joint type="free" name="box_joint" />
</body>
</worldbody>
Expand Down
4 changes: 2 additions & 2 deletions examples/fr3/grasp_demo.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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])),
)
}
Expand Down
4 changes: 2 additions & 2 deletions examples/fr3/grasp_ompl_demo.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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])),
)
}
Expand Down
2 changes: 1 addition & 1 deletion extensions/rcs_taxim/src/rcs_taxim/creators.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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])),
)
}
Expand Down
1 change: 1 addition & 0 deletions python/rcs/__init__.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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",
}
Expand Down
8 changes: 5 additions & 3 deletions python/rcs/envs/base.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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:
Expand Down Expand Up @@ -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()
Expand Down
1 change: 0 additions & 1 deletion python/rcs/envs/configs.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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": {
Expand Down
2 changes: 1 addition & 1 deletion python/rcs/envs/storage_wrapper.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion python/rcs/envs/tasks.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Loading