Interfacing ray casting with geom margins

[markushivers]

Intro

Hi!

I am a graduate student at NTNU, Norway, and I use MuJoCo for my research on SmallSats.

My setup

Current collision detection using ray casting:

def _check_collision(self, point: np.ndarray, direction) -> tuple[bool, float]:
        """
        Checks if a ray from a point in a certain direction 
        intersects with an obstacle.

        Returns distance to collision or -1 if no intersection.
        """
        self.counter += 1
        _ = np.zeros((1, 1), dtype=np.int32)  # Pointer that is needed but unused
        body_exclude_id = self.agent_id + 2
        flg_static = 1  # 0: exclude static geoms, 1: include static geoms
        group_exclusion = None  # Exclude no geom groups
        point = np.array(point, dtype=np.float64)  # Cast to float64
        direction = np.array(direction, dtype=np.float64)  # Cast to float64
        # Calculate distance to collision
        dist = mujoco.mj_ray(self.env.model, self.env.data, point, direction,
                             group_exclusion, flg_static, body_exclude_id, _)

        # Check if collision along the ray
        if dist == -1 or dist > self.resolution*np.linalg.norm(direction):
            return False
        else:
            return True

Geom definition:

<geom mesh="gateway_simple_0" class="collision" margin="0.5" gap="0.5"/>

My question

I am trying to perform path planning in a complex environment whilst avoiding collisions. I have not found an efficient way of checking whether my SmallSat would collide with the environment while going from one waypoint to the next during planning. I have tried using ray casting to check the path's safety, but I'm not able to account for the SmallSat's size while doing that. I was hoping that mj_ray would interface with the margin parameter in each geom, but that doesn't appear to be the case.

I am hoping to get some feedback on feasible alternatives to this approach.

Minimal model and/or code that explain my question

No response

Confirmations

• I searched the latest documentation thoroughly before posting.
• I searched previous Issues and Discussions, I am certain this has not been raised before.

[yuvaltassa]

Did you try using mj_geomDistance?

[yuvaltassa]

Note, you probably want to use nativeccd as you'll get more accurate results.