Skip to content

Commit 027c3e4

Browse files
duburcqaYilingQiao
authored andcommitted
[MISC] Make collision bitmasks filter local to entity for MJCF morphs. (Genesis-Embodied-AI#1499)
* Make collision bitmask filter local to entity for MJCF morphs. * Fix broken 'RigidSolver.links_info.entity_idx'.
1 parent f9463d8 commit 027c3e4

File tree

8 files changed

+48
-20
lines changed

8 files changed

+48
-20
lines changed

examples/collision/contype.py

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -37,8 +37,8 @@ def main():
3737
pos=(0.025, 0, 0.5),
3838
quat=(0, 0, 0, 1),
3939
size=(0.1, 0.1, 0.1),
40-
contype=1,
41-
conaffinity=1,
40+
contype=0b001,
41+
conaffinity=0b001,
4242
),
4343
surface=gs.surfaces.Default(
4444
color=(1.0, 0.0, 0.0, 1.0),
@@ -49,8 +49,8 @@ def main():
4949
pos=(-0.025, 0, 1.0),
5050
quat=(0, 0, 0, 1),
5151
size=(0.1, 0.1, 0.1),
52-
contype=2,
53-
conaffinity=2,
52+
contype=0b010,
53+
conaffinity=0b010,
5454
),
5555
surface=gs.surfaces.Default(
5656
color=(0.0, 1.0, 0.0, 1.0),
@@ -61,8 +61,8 @@ def main():
6161
pos=(0.0, 0, 1.5),
6262
quat=(0, 0, 0, 1),
6363
size=(0.1, 0.1, 0.1),
64-
contype=3,
65-
conaffinity=3,
64+
contype=0b011,
65+
conaffinity=0b011,
6666
),
6767
surface=gs.surfaces.Default(
6868
color=(0.0, 0.0, 1.0, 1.0),
@@ -74,8 +74,8 @@ def main():
7474
scale=0.004,
7575
euler=(0, 0, 90),
7676
pos=(-0.1, 0.0, 1.0),
77-
contype=4,
78-
conaffinity=4,
77+
contype=0b100,
78+
conaffinity=0b100,
7979
),
8080
)
8181

genesis/engine/entities/rigid_entity/rigid_entity.py

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -102,6 +102,7 @@ def _load_model(self):
102102
gs.raise_exception(f"Unsupported morph: {self._morph}.")
103103

104104
self._requires_jac_and_IK = self._morph.requires_jac_and_IK
105+
self._is_local_collision_mask = isinstance(self._morph, gs.morphs.MJCF)
105106

106107
self._update_child_idxs()
107108

@@ -2874,3 +2875,8 @@ def equalities(self):
28742875
def is_free(self):
28752876
"""Whether the entity is free to move."""
28762877
return self._is_free
2878+
2879+
@property
2880+
def is_local_collision_mask(self):
2881+
"""Whether the contype and conaffinity bitmasks of this entity only applies to self-collision."""
2882+
return self._is_local_collision_mask

genesis/engine/entities/rigid_entity/rigid_link.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -54,7 +54,7 @@ def __init__(
5454
self._name: str = name
5555
self._entity: "RigidEntity" = entity
5656
self._solver: "RigidSolver" = entity.solver
57-
self._entity_idx_in_solver = entity.idx
57+
self._entity_idx_in_solver = entity._idx_in_solver
5858

5959
self._uid = gs.UID()
6060
self._idx: int = idx

genesis/engine/scene.py

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -294,9 +294,8 @@ def add_entity(
294294
material = gs.materials.Rigid()
295295

296296
if surface is None:
297-
surface = (
298-
gs.surfaces.Default()
299-
) # assign a local surface, otherwise modification will apply on global default surface
297+
# assign a local surface, otherwise modification will apply on global default surface
298+
surface = gs.surfaces.Default()
300299

301300
if isinstance(material, gs.materials.Rigid):
302301
# small sdf res is sufficient for primitives regardless of size

genesis/engine/solvers/rigid/collider_decomp.py

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -158,13 +158,16 @@ def _compute_collision_pair_validity(self):
158158
links_root_idx = links_root_idx[:, 0]
159159
links_parent_idx = links_parent_idx[:, 0]
160160
links_is_fixed = links_is_fixed[:, 0]
161+
entities_is_local_collision_mask = solver.entities_info.is_local_collision_mask.to_numpy()
161162

162163
n_possible_pairs = 0
163164
collision_pair_validity = np.zeros((n_geoms, n_geoms), dtype=gs.np_int)
164165
for i_ga in range(n_geoms):
165166
for i_gb in range(i_ga + 1, n_geoms):
166167
i_la = geoms_link_idx[i_ga]
167168
i_lb = geoms_link_idx[i_gb]
169+
i_ea = links_entity_idx[i_la]
170+
i_eb = links_entity_idx[i_lb]
168171

169172
# geoms in the same link
170173
if i_la == i_lb:
@@ -182,7 +185,10 @@ def _compute_collision_pair_validity(self):
182185
continue
183186

184187
# contype and conaffinity
185-
if not (
188+
if (
189+
(i_ea == i_eb)
190+
or not (entities_is_local_collision_mask[i_ea] or entities_is_local_collision_mask[i_eb])
191+
) and not (
186192
(geoms_contype[i_ga] & geoms_conaffinity[i_gb]) or (geoms_contype[i_gb] & geoms_conaffinity[i_ga])
187193
):
188194
continue

genesis/engine/solvers/rigid/rigid_solver_decomp.py

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -822,6 +822,9 @@ def _init_entity_fields(self):
822822
entities_gravity_compensation=np.array(
823823
[entity.gravity_compensation for entity in entities], dtype=gs.np_float
824824
),
825+
entities_is_local_collision_mask=np.array(
826+
[entity.is_local_collision_mask for entity in entities], dtype=gs.np_bool
827+
),
825828
# taichi variables
826829
entities_info=self.entities_info,
827830
entities_state=self.entities_state,
@@ -2917,6 +2920,7 @@ def kernel_init_entity_fields(
29172920
entities_geom_start: ti.types.ndarray(),
29182921
entities_geom_end: ti.types.ndarray(),
29192922
entities_gravity_compensation: ti.types.ndarray(),
2923+
entities_is_local_collision_mask: ti.types.ndarray(),
29202924
# taichi variables
29212925
entities_info: array_class.EntitiesInfo,
29222926
entities_state: array_class.EntitiesState,
@@ -2942,6 +2946,7 @@ def kernel_init_entity_fields(
29422946
entities_info.n_geoms[i] = entities_geom_end[i] - entities_geom_start[i]
29432947

29442948
entities_info.gravity_compensation[i] = entities_gravity_compensation[i]
2949+
entities_info.is_local_collision_mask[i] = entities_is_local_collision_mask[i]
29452950

29462951
if ti.static(static_rigid_sim_config.batch_dofs_info):
29472952
for i_d, i_b in ti.ndrange((entities_dof_start[i], entities_dof_end[i]), _B):

genesis/options/morphs.py

Lines changed: 18 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -135,7 +135,6 @@ class Primitive(Morph):
135135
----
136136
This class should *not* be instantiated directly.
137137
138-
139138
Parameters
140139
----------
141140
pos : tuple, shape (3,), optional
@@ -685,13 +684,25 @@ class MJCF(FileMorph):
685684
686685
Note
687686
----
688-
MJCF file always contains a worldbody, which we will skip during loading.
689-
The robots/objects in MJCF come with their own baselink pose.
690-
If `pos`, `euler`, or `quat` is specified, it will override the baselink pose in the MJCF file.
687+
MJCF file always contains a 'world' body. Although this body is added to the kinematic tree, it is used to define
688+
the initial pose of the root link. If `pos`, `euler`, or `quat` is specified, it will override the root pose that
689+
was originally specified in the MJCF file.
690+
691+
Note
692+
----
693+
Genesis currently processes MJCF as if it describing a single entity instead of an actual scene. This means that
694+
there is a single gigantic kinematic chain comprising multiple physical kinematic chains connected together using
695+
fee joints. The definition of kinematic chain has been stretched a bit to allow us. In particular, there must be
696+
multiple root links instead of a single one. One other related limitation is global / world options defined in MJCF
697+
but must be set at the scene-level in Genesis are completely ignored at the moment, e.g. the simulation timestep,
698+
integrator or constraint solver. Building an actual scene hierarchy with multiple independent entities may be
699+
supported in the future.
691700
692-
The current version of Genesis asumes there's only one child of the worldbody.
693-
However, it's possible that a MJCF file contains a scene, not just a single robot, in which case the worldbody will
694-
have multiple kinematic trees. We will support such cases in the future.
701+
Note
702+
----
703+
Collision filters defined in MJCF are considered "local", i.e. they only apply to collision pairs for which both
704+
geometries along to that specific entity. This means that there is no way to filter out collision pairs between
705+
primitive and MJCF entity at the moment.
695706
696707
Parameters
697708
----------

genesis/utils/array_class.py

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1862,6 +1862,7 @@ def get_entities_info(solver):
18621862
"geom_end": V(dtype=gs.ti_int, shape=shape),
18631863
"n_geoms": V(dtype=gs.ti_int, shape=shape),
18641864
"gravity_compensation": V(dtype=gs.ti_float, shape=shape),
1865+
"is_local_collision_mask": V(dtype=gs.ti_bool, shape=shape),
18651866
}
18661867

18671868
if use_ndarray:

0 commit comments

Comments
 (0)