rough terrain support

Author: ooctipus

source/isaaclab/isaaclab/envs/mdp/__init__.pyi

@@ -137,6 +137,8 @@ __all__ = [
     "joint_pos_out_of_manual_limit",
     "joint_vel_out_of_limit",
     "joint_vel_out_of_manual_limit",
+    "body_lin_vel_out_of_limit",
+    "body_ang_vel_out_of_limit",
     "root_height_below_minimum",
     "time_out",
 ]
@@ -285,6 +287,8 @@ from .terminations import (
     joint_pos_out_of_manual_limit,
     joint_vel_out_of_limit,
     joint_vel_out_of_manual_limit,
+    body_lin_vel_out_of_limit,
+    body_ang_vel_out_of_limit,
     root_height_below_minimum,
     time_out,
 )

source/isaaclab/isaaclab/envs/mdp/terminations.py

@@ -150,6 +150,41 @@ def joint_effort_out_of_limit(
     return torch.any(out_of_limits, dim=1)
 
 
+"""
+Body velocity terminations.
+"""
+
+
+def body_lin_vel_out_of_limit(
+    env: ManagerBasedRLEnv,
+    max_velocity: float,
+    asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
+) -> torch.Tensor:
+    """Terminate when any body's linear velocity magnitude exceeds the limit [m/s].
+
+    Also terminates on NaN velocities, which indicate a solver singularity.
+    """
+    asset: Articulation = env.scene[asset_cfg.name]
+    body_vel = wp.to_torch(asset.data.body_lin_vel_w)[:, asset_cfg.body_ids]
+    speed = torch.linalg.norm(body_vel, dim=-1)
+    return torch.any((speed > max_velocity) | torch.isnan(speed), dim=1)
+
+
+def body_ang_vel_out_of_limit(
+    env: ManagerBasedRLEnv,
+    max_velocity: float,
+    asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
+) -> torch.Tensor:
+    """Terminate when any body's angular velocity magnitude exceeds the limit [rad/s].
+
+    Also terminates on NaN velocities, which indicate a solver singularity.
+    """
+    asset: Articulation = env.scene[asset_cfg.name]
+    body_vel = wp.to_torch(asset.data.body_ang_vel_w)[:, asset_cfg.body_ids]
+    speed = torch.linalg.norm(body_vel, dim=-1)
+    return torch.any((speed > max_velocity) | torch.isnan(speed), dim=1)
+
+
 """
 Contact sensor.
 """

source/isaaclab/isaaclab/terrains/terrain_generator.py

@@ -18,7 +18,6 @@
 from isaaclab.utils.timer import Timer
 from isaaclab.utils.warp import convert_to_warp_mesh
 
-from .trimesh.utils import make_border
 from .utils import color_meshes_by_height, find_flat_patches
 
 if TYPE_CHECKING:
@@ -270,25 +269,48 @@ def _generate_curriculum_terrains(self):
     """
 
     def _add_terrain_border(self):
-        """Add a surrounding border over all the sub-terrains into the terrain meshes."""
-        # border parameters
-        border_size = (
-            self.cfg.num_rows * self.cfg.size[0] + 2 * self.cfg.border_width,
-            self.cfg.num_cols * self.cfg.size[1] + 2 * self.cfg.border_width,
-        )
-        inner_size = (self.cfg.num_rows * self.cfg.size[0], self.cfg.num_cols * self.cfg.size[1])
-        border_center = (
-            self.cfg.num_rows * self.cfg.size[0] / 2,
-            self.cfg.num_cols * self.cfg.size[1] / 2,
-            -self.cfg.border_height / 2,
-        )
-        # border mesh
-        border_meshes = make_border(border_size, inner_size, height=abs(self.cfg.border_height), position=border_center)
-        border = trimesh.util.concatenate(border_meshes)
-        # update the faces to have minimal triangles
-        selector = ~(np.asarray(border.triangles)[:, :, 2] < -0.1).any(1)
-        border.update_faces(selector)
-        # add the border to the list of meshes
+        """Add a surrounding border over all the sub-terrains into the terrain meshes.
+
+        The border is built as a subdivided grid mesh (matching ``horizontal_scale``)
+        rather than large box primitives.  Large triangles on the border can cause
+        collision detection failures in some physics backends (e.g. Newton/MuJoCo).
+        """
+        bw = self.cfg.border_width
+        if bw <= 0:
+            return
+
+        inner_w = self.cfg.num_rows * self.cfg.size[0]
+        inner_l = self.cfg.num_cols * self.cfg.size[1]
+        hs = self.cfg.horizontal_scale
+
+        def _make_grid_strip(x0: float, y0: float, width: float, length: float) -> trimesh.Trimesh:
+            """Create a flat subdivided mesh strip at z=0 with grid spacing ``hs``."""
+            nx = max(int(np.ceil(width / hs)), 1) + 1
+            ny = max(int(np.ceil(length / hs)), 1) + 1
+            xs = np.linspace(x0, x0 + width, nx)
+            ys = np.linspace(y0, y0 + length, ny)
+            xx, yy = np.meshgrid(xs, ys, indexing="ij")
+            vertices = np.zeros((nx * ny, 3))
+            vertices[:, 0] = xx.flatten()
+            vertices[:, 1] = yy.flatten()
+            faces = []
+            for i in range(nx - 1):
+                for j in range(ny - 1):
+                    v0 = i * ny + j
+                    v1 = v0 + 1
+                    v2 = (i + 1) * ny + j
+                    v3 = v2 + 1
+                    faces.append([v0, v2, v1])
+                    faces.append([v1, v2, v3])
+            return trimesh.Trimesh(vertices=vertices, faces=np.array(faces, dtype=np.uint32))
+
+        strips = [
+            _make_grid_strip(-bw, -bw, inner_w + 2 * bw, bw),  # bottom
+            _make_grid_strip(-bw, inner_l, inner_w + 2 * bw, bw),  # top
+            _make_grid_strip(-bw, 0.0, bw, inner_l),  # left
+            _make_grid_strip(inner_w, 0.0, bw, inner_l),  # right
+        ]
+        border = trimesh.util.concatenate(strips)
         self.terrain_meshes.append(border)
 
     def _add_sub_terrain(

source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/rough_env_cfg.py

@@ -3,13 +3,13 @@
 #
 # SPDX-License-Identifier: BSD-3-Clause
 
+from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg, NewtonCollisionPipelineCfg
+from isaaclab_physx.physics import PhysxCfg
+
+from isaaclab.sim import SimulationCfg
 from isaaclab.utils import configclass
 
-from isaaclab_tasks.manager_based.locomotion.velocity.velocity_env_cfg import (
-    EventsCfg,
-    LocomotionVelocityRoughEnvCfg,
-    StartupEventsCfg,
-)
+from isaaclab_tasks.manager_based.locomotion.velocity.velocity_env_cfg import LocomotionVelocityRoughEnvCfg
 from isaaclab_tasks.utils import PresetCfg
 
 ##
@@ -19,20 +19,27 @@
 
 
 @configclass
-class AnymalDPhysxEventsCfg(EventsCfg, StartupEventsCfg):
-    pass
-
-
-@configclass
-class AnymalDEventsCfg(PresetCfg):
-    default = AnymalDPhysxEventsCfg()
-    newton = EventsCfg()
+class RoughPhysicsCfg(PresetCfg):
+    default = PhysxCfg(gpu_max_rigid_patch_count=10 * 2**15)
+    newton = NewtonCfg(
+        solver_cfg=MJWarpSolverCfg(
+            njmax=200,
+            nconmax=100,
+            cone="pyramidal",
+            impratio=1.0,
+            integrator="implicitfast",
+            use_mujoco_contacts=False,
+        ),
+        collision_cfg=NewtonCollisionPipelineCfg(max_triangle_pairs=2_500_000),
+        num_substeps=1,
+        debug_mode=False,
+    )
     physx = default
 
 
 @configclass
 class AnymalDRoughEnvCfg(LocomotionVelocityRoughEnvCfg):
-    events: AnymalDEventsCfg = AnymalDEventsCfg()
+    sim: SimulationCfg = SimulationCfg(physics=RoughPhysicsCfg())
 
     def __post_init__(self):
         # post init of parent

source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py

@@ -27,7 +27,7 @@
 from isaaclab.utils.noise import UniformNoiseCfg as Unoise
 
 import isaaclab_tasks.manager_based.locomotion.velocity.mdp as mdp
-from isaaclab_tasks.utils import PresetCfg
+from isaaclab_tasks.utils import PresetCfg, preset
 
 ##
 # Pre-defined configs
@@ -161,6 +161,50 @@ def __post_init__(self):
 class EventsCfg:
     """Configuration for events."""
 
+    # startup
+    physics_material = EventTerm(
+        func=mdp.randomize_rigid_body_material,
+        mode="startup",
+        params={
+            "asset_cfg": SceneEntityCfg("robot", body_names=".*"),
+            "static_friction_range": (0.8, 0.8),
+            "dynamic_friction_range": (0.6, 0.6),
+            "restitution_range": (0.0, 0.0),
+            "num_buckets": 64,
+        },
+    )
+
+    add_base_mass = EventTerm(
+        func=mdp.randomize_rigid_body_mass,
+        mode="startup",
+        params={
+            "asset_cfg": SceneEntityCfg("robot", body_names="base"),
+            "mass_distribution_params": (-5.0, 5.0),
+            "operation": "add",
+        },
+    )
+
+    base_com = preset(
+        default=EventTerm(
+            func=mdp.randomize_rigid_body_com,
+            mode="startup",
+            params={
+                "asset_cfg": SceneEntityCfg("robot", body_names="base"),
+                "com_range": {"x": (-0.05, 0.05), "y": (-0.05, 0.05), "z": (-0.01, 0.01)},
+            },
+        ),
+        newton=None,
+    )
+
+    collider_offsets = EventTerm(
+        func=mdp.randomize_rigid_body_collider_offsets,
+        mode="startup",
+        params={
+            "asset_cfg": SceneEntityCfg("robot", body_names=".*"),
+            "contact_offset_distribution_params": (0.01, 0.01),
+        },
+    )
+
     # reset
     base_external_force_torque = EventTerm(
         func=mdp.apply_external_force_torque,
@@ -206,41 +250,6 @@ class EventsCfg:
     )
 
 
-@configclass
-class StartupEventsCfg:
-    # startup
-    physics_material = EventTerm(
-        func=mdp.randomize_rigid_body_material,
-        mode="startup",
-        params={
-            "asset_cfg": SceneEntityCfg("robot", body_names=".*"),
-            "static_friction_range": (0.8, 0.8),
-            "dynamic_friction_range": (0.6, 0.6),
-            "restitution_range": (0.0, 0.0),
-            "num_buckets": 64,
-        },
-    )
-
-    add_base_mass = EventTerm(
-        func=mdp.randomize_rigid_body_mass,
-        mode="startup",
-        params={
-            "asset_cfg": SceneEntityCfg("robot", body_names="base"),
-            "mass_distribution_params": (-5.0, 5.0),
-            "operation": "add",
-        },
-    )
-
-    base_com = EventTerm(
-        func=mdp.randomize_rigid_body_com,
-        mode="startup",
-        params={
-            "asset_cfg": SceneEntityCfg("robot", body_names="base"),
-            "com_range": {"x": (-0.05, 0.05), "y": (-0.05, 0.05), "z": (-0.01, 0.01)},
-        },
-    )
-
-
 @configclass
 class RewardsCfg:
     """Reward terms for the MDP."""
@@ -286,6 +295,10 @@ class TerminationsCfg:
         func=mdp.illegal_contact,
         params={"sensor_cfg": SceneEntityCfg("contact_forces", body_names="base"), "threshold": 1.0},
     )
+    body_lin_vel = DoneTerm(
+        func=mdp.body_lin_vel_out_of_limit,
+        params={"max_velocity": 20.0, "asset_cfg": SceneEntityCfg("robot", body_names=".*")},
+    )
 
 
 @configclass
@@ -313,7 +326,7 @@ class LocomotionVelocityRoughEnvCfg(ManagerBasedRLEnvCfg):
     # MDP settings
     rewards: RewardsCfg = RewardsCfg()
     terminations: TerminationsCfg = TerminationsCfg()
-    events: EventsCfg = MISSING
+    events: EventsCfg = EventsCfg()
     curriculum: CurriculumCfg = CurriculumCfg()
 
     def __post_init__(self):