diff --git a/src/jaxsim/terrain/terrain.py b/src/jaxsim/terrain/terrain.py index b139f7db2..9afd19461 100644 --- a/src/jaxsim/terrain/terrain.py +++ b/src/jaxsim/terrain/terrain.py @@ -147,7 +147,7 @@ def height(self, x: jtp.FloatLike, y: jtp.FloatLike) -> jtp.Float: D = -C * self._height # Invert the plane equation to get the height at the given (x, y) coordinates. - return jnp.array(-(A * x + B * y + D) / C).astype(float) + return jnp.array(-(A * x + B * y + D) / (C + 1e-9)).astype(float) def __hash__(self) -> int: