Skip to content

Commit c88b62d

Browse files
juelgjuelg
authored
Merge pull request #336 from RobotControlStack/juelg/fix-penetration
fix(sim): penetration depth on collision
2 parents f56be05 + 4a9bb13 commit c88b62d

11 files changed

Lines changed: 29 additions & 13 deletions

File tree

‎assets/objects/green_cube/green_cube.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@
66

77
<worldbody>
88
<body name="box_body">
9-
<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" />
9+
<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" />
1010
<joint type="free" name="box_joint" />
1111
</body>
1212
</worldbody>

‎assets/objects/green_cuboid/green_cuboid.xml‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
<mujoco model="green_cuboid">
2+
<compiler angle="radian" meshdir="assets" autolimits="true"/>
3+
4+
<option cone="elliptic" impratio="10"/>
5+
6+
7+
<worldbody>
8+
<body name="box_body">
9+
<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" />
10+
<joint type="free" name="box_joint" />
11+
</body>
12+
</worldbody>
13+
14+
</mujoco>

‎assets/objects/red_cube/red_cube.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@
66

77
<worldbody>
88
<body name="box_body">
9-
<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" />
9+
<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" />
1010
<joint type="free" name="box_joint" />
1111
</body>
1212
</worldbody>

‎examples/fr3/grasp_demo.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -92,8 +92,8 @@ def main():
9292
cfg.sim_cfg.async_control = True
9393
cfg.max_relative_movement = None
9494
cfg.root_frame_objects = {
95-
"green_cube": (
96-
rcs.OBJECT_PATHS["green_cube"],
95+
"green_cuboid": (
96+
rcs.OBJECT_PATHS["green_cuboid"],
9797
Pose(translation=np.array([0.5, 0.0, 0.05]), quaternion=np.array([0.0, 0.0, 0.0, 1.0])),
9898
)
9999
}

‎examples/fr3/grasp_ompl_demo.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -120,8 +120,8 @@ def main():
120120
cfg.sim_cfg.async_control = True
121121
cfg.max_relative_movement = None
122122
cfg.root_frame_objects = {
123-
"green_cube": (
124-
rcs.OBJECT_PATHS["green_cube"],
123+
"green_cuboid": (
124+
rcs.OBJECT_PATHS["green_cuboid"],
125125
Pose(translation=np.array([0.5, 0.0, 0.05]), quaternion=np.array([0.0, 0.0, 0.0, 1.0])),
126126
)
127127
}

‎extensions/rcs_taxim/src/rcs_taxim/creators.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -102,7 +102,7 @@ def __call__(
102102
cfg.gripper_offsets = {"right": rcs.GRIPPER_MOUNT_OFFSETS[rcs.common.GripperType("Robotiq2F85")]}
103103
cfg.root_frame_objects = {
104104
"": (
105-
rcs.OBJECT_PATHS["green_cube"],
105+
rcs.OBJECT_PATHS["green_cuboid"],
106106
Pose(translation=np.array([0.5, 0.0, 0.05]), quaternion=np.array([0.0, 0.0, 0.0, 1.0])),
107107
)
108108
}

‎python/rcs/__init__.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -226,6 +226,7 @@ class RobotMetaConfig:
226226
"fr3_single_mount": "assets/objects/fr3_single_mount/fr3_single_mount.xml",
227227
"robotiq_d405_mount": "assets/objects/robotiq_d405_mount/robotiq_d405_mount.xml",
228228
"droid_wrist_mount": "assets/objects/droid_wrist_mount/droid_wrist_mount.xml",
229+
"green_cuboid": "assets/objects/green_cuboid/green_cuboid.xml",
229230
"green_cube": "assets/objects/green_cube/green_cube.xml",
230231
"red_cube": "assets/objects/red_cube/red_cube.xml",
231232
}

‎python/rcs/envs/base.py‎

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1042,10 +1042,11 @@ def close(self):
10421042
class GripperWrapper(ActObsInfoWrapper):
10431043
# TODO: sticky gripper, like in aloha
10441044

1045+
GRIPPER_THRESHOLD = 0.5
10451046
BINARY_GRIPPER_CLOSED: ClassVar[list[float]] = [0]
10461047
BINARY_GRIPPER_OPEN: ClassVar[list[float]] = [1]
10471048

1048-
def __init__(self, env, gripper: common.Gripper, binary: bool = True):
1049+
def __init__(self, env, gripper: common.Gripper, binary: bool = True, prev_action_obs: bool = False):
10491050
super().__init__(env)
10501051
self.binary = binary
10511052
self.observation_space: gym.spaces.Dict
@@ -1055,6 +1056,7 @@ def __init__(self, env, gripper: common.Gripper, binary: bool = True):
10551056
self.gripper_key = get_space_keys(GripperDictType)[0]
10561057
self.gripper = gripper
10571058
self._last_gripper_cmd = None
1059+
self.prev_action_obs = prev_action_obs
10581060

10591061
def _command_changed(self, gripper_action: np.ndarray) -> bool:
10601062
if self._last_gripper_cmd is None:
@@ -1098,8 +1100,8 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]:
10981100
gripper_action = np.clip(np.asarray(gripper_action, dtype=np.float32), 0.0, 1.0)
10991101

11001102
if self._command_changed(gripper_action):
1101-
if self.binary:
1102-
self.gripper.grasp() if gripper_action[0] < 0.5 else self.gripper.open()
1103+
if self.prev_action_obs:
1104+
self.gripper.grasp() if gripper_action[0] < self.GRIPPER_THRESHOLD else self.gripper.open()
11031105
else:
11041106
self.gripper.set_normalized_width(float(gripper_action[0]))
11051107
self._last_gripper_cmd = gripper_action.tolist()

‎python/rcs/envs/configs.py‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -392,7 +392,6 @@ def config(self) -> SimEnvCreatorConfig:
392392
world_frame_objects: dict[str, tuple[str, rcs.common.Pose]] | None = None
393393
root_frame_objects: dict[str, tuple[str, rcs.common.Pose]] | None = {
394394
"duo_mount": (OBJECT_PATHS["fr3_duo_mount"], DEFAULT_TRANSFORMS["FR3_DUOMOUNT_BASE"]),
395-
# "green_cube": (OBJECT_PATHS["green_cube"], Pose(translation=[0.5, 0, 0.5], quaternion=[0, 0, 0, 1])),
396395
}
397396
robot_frame_objects: dict[str, dict[str, tuple[str, rcs.common.Pose]]] | None = {
398397
"left": {

‎python/rcs/envs/storage_wrapper.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -329,7 +329,7 @@ def reset(self, *, seed: int | None = None, options: dict[str, Any] | None = Non
329329
self._success = False
330330
self._prev_action = None
331331
self._prev_absolute_action = None
332-
obs, info = self.env.reset()
332+
obs, info = self.env.reset(seed=seed, options=options)
333333
self.step_cnt = 0
334334
self.uuid = uuid4()
335335
return obs, info

0 commit comments

Comments
 (0)