diff --git a/genesis/engine/solvers/rigid/collider_decomp.py b/genesis/engine/solvers/rigid/collider_decomp.py index 66d8c995bc..91da9e3f79 100644 --- a/genesis/engine/solvers/rigid/collider_decomp.py +++ b/genesis/engine/solvers/rigid/collider_decomp.py @@ -572,16 +572,14 @@ def _func_contact_mpr_terrain(self, i_ga, i_gb, i_b): ) ) - for i in range(6): - i_axis = i % 3 - i_m = i // 3 - - sign = gs.ti_float(1 - i_m * 2) - direction = ti.Vector([i_axis == 0, i_axis == 1, i_axis == 2], dt=gs.ti_float) - direction = direction * sign - + for i_axis, i_m in ti.ndrange(3, 2): + direction = ti.Vector.zero(gs.ti_float, 3) + if i_m == 0: + direction[i_axis] = 1.0 + else: + direction[i_axis] = -1.0 v1 = self._mpr.support_driver(direction, i_ga, i_b) - self.xyz_max_min[i, i_b] = v1[i_axis] + self.xyz_max_min[3 * i_m + i_axis, i_b] = v1[i_axis] for i in ti.static(range(3)): self.prism[i, i_b][2] = self._solver.terrain_xyz_maxmin[5] @@ -622,24 +620,27 @@ def _func_contact_mpr_terrain(self, i_ga, i_gb, i_b): or self.prism[4, i_b][2] >= self.xyz_max_min[5, i_b] or self.prism[5, i_b][2] >= self.xyz_max_min[5, i_b] ): - pos = ti.Vector.zero(gs.ti_float, 3) + center_a = gu.ti_transform_by_trans_quat( + self._solver.geoms_info[i_ga].center, ga_pos, ga_quat + ) + center_b = ti.Vector.zero(gs.ti_float, 3) for i_p in ti.static(range(6)): - pos = pos + self.prism[i_p, i_b] + center_b = center_b + self.prism[i_p, i_b] + center_b = center_b / 6.0 - self._solver.geoms_info[i_gb].center = pos / 6 self._solver.geoms_state[i_gb, i_b].pos = ti.Vector.zero(gs.ti_float, 3) self._solver.geoms_state[i_gb, i_b].quat = gu.ti_identity_quat() - is_col, normal, penetration, contact_pos = self._mpr.func_mpr_contact( - i_ga, i_gb, i_b, ti.Vector.zero(gs.ti_float, 3) + is_col, normal, penetration, contact_pos = self._mpr.func_mpr_contact_from_centers( + i_ga, i_gb, i_b, center_a, center_b ) if is_col: normal = gu.ti_transform_by_quat(normal, gb_quat) contact_pos = gu.ti_transform_by_quat(contact_pos, gb_quat) contact_pos = contact_pos + gb_pos - i_col = self.n_contacts[i_b] valid = True + i_col = self.n_contacts[i_b] for j in range(cnt): if ( contact_pos - self.contact_data[i_col - j - 1, i_b].pos diff --git a/genesis/engine/solvers/rigid/mpr_decomp.py b/genesis/engine/solvers/rigid/mpr_decomp.py index 66b2f4361f..c83d20bf64 100644 --- a/genesis/engine/solvers/rigid/mpr_decomp.py +++ b/genesis/engine/solvers/rigid/mpr_decomp.py @@ -357,76 +357,7 @@ def mpr_expand_portal(self, v, v1, v2, i_ga, i_gb, i_b): self.simplex_support[i_s, i_b].v = v @ti.func - def mpr_discover_portal(self, i_ga, i_gb, i_b, normal_ws): - # MPR algorithm was initially design to check whether a pair of convex geometries was colliding. The author - # proposed to extend its application to collision detection as it can provide the contact normal and penetration - # depth in some cases, i.e. when the original of the Minkowski difference can be projected inside the refined - # portal. Beyond this specific scenario, it only provides an approximation, that gets worst and worst as the - # ray casting and portal normal are misaligned. - # For convex shape, one can show that everything should be fine for low penetration-to-size ratio for each - # geometry, and the probability to accurately estimate the contact point decreases as this ratio increases. - # - # This issue can be avoided by initializing the algorithm with the good seach direction, basically the one - # from the previous simulation timestep would do fine, as the penetration was smaller at that time and so the - # likely for this direction to be valid was larger. Alternatively, the direction of the linear velocity would - # be a good option. - # - # Enforcing a specific search direction to vanilla MPR is not straightforward, because the direction of the ray - # control by v0, which is defined as the difference between the respective centers of each geometry. - # The only option is to change the way the center of each geometry are defined, so as to make the ray casting - # from origin to v0 as colinear as possible with the direction we are interested, while remaining included in - # their respective geometry. - # The idea is to offset the original centers of each geometry by a ratio that corresponds to their respective - # (rotated) bounding box size along each axe. Each center cannot be moved more than half of its bound-box size - # along each axe. This could lead to a center that is outside the geometries if they do not collide, but - # should be fine otherwise. Anyway, this is not a big deal in practice and MPR is robust enough to converge to - # a meaningful solution and if the center is slightly off of each geometry. Nevertheless, if it turns out this - # is a real issue, one way to address it is to evaluate the exact signed distance of each center wrt their - # respective geometry. If one of the center is off, its offset from the original center is divided by 2 and the - # signed distance is computed once again until to find a valid point. This procedure should be cheap. - - g_state_a = self._solver.geoms_state[i_ga, i_b] - g_state_b = self._solver.geoms_state[i_gb, i_b] - g_info = self._solver.geoms_info[i_ga] - center_a = gu.ti_transform_by_trans_quat(g_info.center, g_state_a.pos, g_state_a.quat) - g_info = self._solver.geoms_info[i_gb] - center_b = gu.ti_transform_by_trans_quat(g_info.center, g_state_b.pos, g_state_b.quat) - - # Completely different center logics if a normal guess is provided - if ti.static(not self._solver._enable_mujoco_compatibility): - if (ti.abs(normal_ws) > self.CCD_EPS).any(): - # Must start from the center of each bounding box - center_a_local = 0.5 * (self._solver.geoms_init_AABB[i_ga, 7] + self._solver.geoms_init_AABB[i_ga, 0]) - center_a = gu.ti_transform_by_trans_quat(center_a_local, g_state_a.pos, g_state_a.quat) - center_b_local = 0.5 * (self._solver.geoms_init_AABB[i_gb, 7] + self._solver.geoms_init_AABB[i_gb, 0]) - center_b = gu.ti_transform_by_trans_quat(center_b_local, g_state_b.pos, g_state_b.quat) - delta = center_a - center_b - - # Skip offset if normal is roughly pointing in the same direction already. - # Note that a threshold of 0.5 would probably make more sense, but this means that the center of each - # geometry would significantly affect collision detection, which is undesirable. - normal = delta.normalized() - if normal_ws.cross(normal).norm() > 0.01: - # Compute the target offset - offset = delta.dot(normal_ws) * normal_ws - delta - offset_norm = offset.norm() - - if offset_norm > gs.EPS: - # Compute the size of the bounding boxes along the target offset direction. - # First, move the direction in local box frame - dir_offset = offset / offset_norm - dir_offset_local_a = gu.ti_inv_transform_by_quat(dir_offset, g_state_a.quat) - dir_offset_local_b = gu.ti_inv_transform_by_quat(dir_offset, g_state_b.quat) - box_size_a = self._solver.geoms_init_AABB[i_ga, 7] - self._solver.geoms_init_AABB[i_ga, 0] - box_size_b = self._solver.geoms_init_AABB[i_gb, 7] - self._solver.geoms_init_AABB[i_gb, 0] - length_a = box_size_a.dot(ti.abs(dir_offset_local_a)) - length_b = box_size_b.dot(ti.abs(dir_offset_local_b)) - - # Shift the center of each geometry - offset_ratio = ti.min(offset_norm / (length_a + length_b), 0.5) - center_a = center_a + dir_offset * length_a * offset_ratio - center_b = center_b - dir_offset * length_b * offset_ratio - + def mpr_discover_portal(self, i_ga, i_gb, i_b, center_a, center_b): self.simplex_support[0, i_b].v1 = center_a self.simplex_support[0, i_b].v2 = center_b self.simplex_support[0, i_b].v = center_a - center_b @@ -526,8 +457,81 @@ def mpr_discover_portal(self, i_ga, i_gb, i_b, normal_ws): return ret @ti.func - def func_mpr_contact(self, i_ga, i_gb, i_b, normal_ws): - res = self.mpr_discover_portal(i_ga, i_gb, i_b, normal_ws) + def guess_geoms_center(self, i_ga, i_gb, i_b, normal_ws): + # MPR algorithm was initially design to check whether a pair of convex geometries was colliding. The author + # proposed to extend its application to collision detection as it can provide the contact normal and penetration + # depth in some cases, i.e. when the original of the Minkowski difference can be projected inside the refined + # portal. Beyond this specific scenario, it only provides an approximation, that gets worst and worst as the + # ray casting and portal normal are misaligned. + # For convex shape, one can show that everything should be fine for low penetration-to-size ratio for each + # geometry, and the probability to accurately estimate the contact point decreases as this ratio increases. + # + # This issue can be avoided by initializing the algorithm with the good seach direction, basically the one + # from the previous simulation timestep would do fine, as the penetration was smaller at that time and so the + # likely for this direction to be valid was larger. Alternatively, the direction of the linear velocity would + # be a good option. + # + # Enforcing a specific search direction to vanilla MPR is not straightforward, because the direction of the ray + # control by v0, which is defined as the difference between the respective centers of each geometry. + # The only option is to change the way the center of each geometry are defined, so as to make the ray casting + # from origin to v0 as colinear as possible with the direction we are interested, while remaining included in + # their respective geometry. + # The idea is to offset the original centers of each geometry by a ratio that corresponds to their respective + # (rotated) bounding box size along each axe. Each center cannot be moved more than half of its bound-box size + # along each axe. This could lead to a center that is outside the geometries if they do not collide, but + # should be fine otherwise. Anyway, this is not a big deal in practice and MPR is robust enough to converge to + # a meaningful solution and if the center is slightly off of each geometry. Nevertheless, if it turns out this + # is a real issue, one way to address it is to evaluate the exact signed distance of each center wrt their + # respective geometry. If one of the center is off, its offset from the original center is divided by 2 and the + # signed distance is computed once again until to find a valid point. This procedure should be cheap. + + g_state_a = self._solver.geoms_state[i_ga, i_b] + g_state_b = self._solver.geoms_state[i_gb, i_b] + g_info = self._solver.geoms_info[i_ga] + center_a = gu.ti_transform_by_trans_quat(g_info.center, g_state_a.pos, g_state_a.quat) + g_info = self._solver.geoms_info[i_gb] + center_b = gu.ti_transform_by_trans_quat(g_info.center, g_state_b.pos, g_state_b.quat) + + # Completely different center logics if a normal guess is provided + if ti.static(not self._solver._enable_mujoco_compatibility): + if (ti.abs(normal_ws) > self.CCD_EPS).any(): + # Must start from the center of each bounding box + center_a_local = 0.5 * (self._solver.geoms_init_AABB[i_ga, 7] + self._solver.geoms_init_AABB[i_ga, 0]) + center_a = gu.ti_transform_by_trans_quat(center_a_local, g_state_a.pos, g_state_a.quat) + center_b_local = 0.5 * (self._solver.geoms_init_AABB[i_gb, 7] + self._solver.geoms_init_AABB[i_gb, 0]) + center_b = gu.ti_transform_by_trans_quat(center_b_local, g_state_b.pos, g_state_b.quat) + delta = center_a - center_b + + # Skip offset if normal is roughly pointing in the same direction already. + # Note that a threshold of 0.5 would probably make more sense, but this means that the center of each + # geometry would significantly affect collision detection, which is undesirable. + normal = delta.normalized() + if normal_ws.cross(normal).norm() > 0.01: + # Compute the target offset + offset = delta.dot(normal_ws) * normal_ws - delta + offset_norm = offset.norm() + + if offset_norm > gs.EPS: + # Compute the size of the bounding boxes along the target offset direction. + # First, move the direction in local box frame + dir_offset = offset / offset_norm + dir_offset_local_a = gu.ti_inv_transform_by_quat(dir_offset, g_state_a.quat) + dir_offset_local_b = gu.ti_inv_transform_by_quat(dir_offset, g_state_b.quat) + box_size_a = self._solver.geoms_init_AABB[i_ga, 7] - self._solver.geoms_init_AABB[i_ga, 0] + box_size_b = self._solver.geoms_init_AABB[i_gb, 7] - self._solver.geoms_init_AABB[i_gb, 0] + length_a = box_size_a.dot(ti.abs(dir_offset_local_a)) + length_b = box_size_b.dot(ti.abs(dir_offset_local_b)) + + # Shift the center of each geometry + offset_ratio = ti.min(offset_norm / (length_a + length_b), 0.5) + center_a = center_a + dir_offset * length_a * offset_ratio + center_b = center_b - dir_offset * length_b * offset_ratio + + return center_a, center_b + + @ti.func + def func_mpr_contact_from_centers(self, i_ga, i_gb, i_b, center_a, center_b): + res = self.mpr_discover_portal(i_ga, i_gb, i_b, center_a, center_b) is_col = False pos = gs.ti_vec3([0.0, 0.0, 0.0]) @@ -544,3 +548,8 @@ def func_mpr_contact(self, i_ga, i_gb, i_b, normal_ws): is_col, normal, penetration, pos = self.mpr_find_penetration(i_ga, i_gb, i_b) return is_col, normal, penetration, pos + + @ti.func + def func_mpr_contact(self, i_ga, i_gb, i_b, normal_ws): + center_a, center_b = self.guess_geoms_center(i_ga, i_gb, i_b, normal_ws) + return self.func_mpr_contact_from_centers(i_ga, i_gb, i_b, center_a, center_b) diff --git a/tests/test_rigid_physics.py b/tests/test_rigid_physics.py index 855ca61586..2ef9c27c9c 100644 --- a/tests/test_rigid_physics.py +++ b/tests/test_rigid_physics.py @@ -2007,6 +2007,7 @@ def test_terrain_generation(show_viewer): camera_fov=40, ), show_viewer=show_viewer, + show_FPS=False, ) terrain = scene.add_entity( morph=gs.morphs.Terrain(