diff --git a/docs/guide/configuration.rst b/docs/guide/configuration.rst index 4061b30ff..6a12773da 100644 --- a/docs/guide/configuration.rst +++ b/docs/guide/configuration.rst @@ -17,6 +17,10 @@ Environment variables starting with ``JAXSIM_COLLISION_`` are used to configure *Default:* ``False``. +- ``JAXSIM_COLLISION_INCLUDE_OVERLAPS``: Enables or disables the inclusion of overlapping collision points. + + *Default:* ``False``. + - ``JAXSIM_COLLISION_USE_BOTTOM_ONLY``: Limits collision detection to only the bottom half of the box or sphere. *Default:* ``False``. diff --git a/src/jaxsim/api/kin_dyn_parameters.py b/src/jaxsim/api/kin_dyn_parameters.py index 8d78d0eff..41764823a 100644 --- a/src/jaxsim/api/kin_dyn_parameters.py +++ b/src/jaxsim/api/kin_dyn_parameters.py @@ -1,6 +1,7 @@ from __future__ import annotations import dataclasses +import os import jax.lax import jax.numpy as jnp @@ -797,18 +798,22 @@ def build_from(model_description: ModelDescription) -> ContactParameters: collidable_points = model_description.all_enabled_collidable_points() # Extract the positions L_p_C of the collidable points w.r.t. the link frames - # they are rigidly attached to. We exclude overlapping points. - points, idxs = jnp.unique( - jnp.vstack([cp.position for cp in collidable_points]), - axis=0, - return_index=True, - ) + # they are rigidly attached to. + all_points = jnp.vstack([cp.position for cp in collidable_points]) + + if os.environ.get("JAXSIM_COLLISION_INCLUDE_OVERLAPS", "0").lower() in { + "false", + "0", + }: + points, idxs = jnp.unique(all_points, axis=0, return_index=True) + selected_points = map(collidable_points.__getitem__, idxs) + else: + points = all_points + selected_points = iter(collidable_points) - # Extract the indices of the links to which the collidable points are rigidly - # attached to. + # Extract the indices of the links to which the collidable points are rigidly attached. link_index_of_points = tuple( - links_dict[cp.parent_link.name].index - for cp in map(collidable_points.__getitem__, idxs) + map(lambda cp: links_dict[cp.parent_link.name].index, selected_points) ) # Build the ContactParameters object.