Updated navigation tasks to support Newton + minor bug fixes

Minor fix

Implemented obstacle generation and newton camera more efficiently

Minor fix

Minor fix

Author: umenzi

source/isaaclab_contrib/isaaclab_contrib/controllers/lee_controller_base.py

@@ -55,10 +55,10 @@ def __init__(self, cfg: LeeControllerBaseCfg, asset: Multirotor, num_envs: int,
         root_quat_exp = root_quat_w.unsqueeze(1).expand(num_envs, self.robot.num_bodies, 4)
         body_link_pos_delta = body_link_pos_w - root_pos_w.unsqueeze(1)
 
-        body_masses = self._to_torch(self.robot.root_view.get_masses())
+        body_masses = self._to_torch(self.robot.data.body_mass)
         body_inv_mass_local = torch.where(body_masses > 0, 1.0 / body_masses, torch.zeros_like(body_masses))
         self.mass, self.robot_inertia, _ = aggregate_inertia_about_robot_com(
-            self._to_torch(self.robot.root_view.get_inertias()),
+            self._to_torch(self.robot.data.body_inertia),
             body_inv_mass_local,
             body_com_pos_b,
             body_com_quat_b,

source/isaaclab_contrib/test/controllers/test_drone_geometric_controllers.py

@@ -36,27 +36,13 @@
 from isaaclab_contrib.controllers.lee_velocity_control_cfg import LeeVelControllerCfg
 
 
-class _DummyRootView:
-    """Stub articulation view with ``get_masses`` and ``get_inertias`` for controller tests."""
-
-    def __init__(self, num_envs: int, num_bodies: int, device: torch.device):
-        inertia_flat = torch.eye(3, device=device).reshape(9)
-        self._inertias = inertia_flat.unsqueeze(0).unsqueeze(0).expand(num_envs, num_bodies, 9).clone()
-        self._masses = torch.ones((num_envs, num_bodies), device=device)
-
-    def get_inertias(self) -> torch.Tensor:
-        return self._inertias
-
-    def get_masses(self) -> torch.Tensor:
-        return self._masses
-
-
 class _DummyRobot:
     """Minimal multirotor stub exposing the attributes used by the controllers."""
 
     def __init__(self, num_envs: int, num_bodies: int, device: torch.device):
         self.num_bodies = num_bodies
         quat_id = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device)
+        inertia_flat = torch.eye(3, device=device).reshape(9)
         self.data = types.SimpleNamespace(
             root_link_quat_w=quat_id.repeat(num_envs, 1),
             root_quat_w=quat_id.repeat(num_envs, 1),
@@ -67,8 +53,9 @@ def __init__(self, num_envs: int, num_bodies: int, device: torch.device):
             body_link_quat_w=quat_id.repeat(num_envs, num_bodies, 1),
             body_com_pos_b=torch.zeros((num_envs, num_bodies, 3), device=device),
             body_com_quat_b=quat_id.repeat(num_envs, num_bodies, 1),
+            body_mass=torch.ones((num_envs, num_bodies), device=device),
+            body_inertia=inertia_flat.unsqueeze(0).unsqueeze(0).expand(num_envs, num_bodies, 9).clone(),
         )
-        self.root_view = _DummyRootView(num_envs, num_bodies, device)
 
 
 class _DummySimCfg:

source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/events.py

@@ -40,8 +40,8 @@ def reset_obstacles_with_individual_ranges(
 
     Walls are positioned at fixed locations based on their configuration ratios. Obstacles
     are randomly placed within their designated zones, with the number of active obstacles
-    determined by the curriculum difficulty level. Inactive obstacles are moved far below
-    the scene (-1000m in Z) to effectively remove them from the environment.
+    determined by the curriculum difficulty level. Inactive obstacles are parked at distinct
+    locations far below the scene to avoid overlapping collision geometry.
 
     The curriculum scaling works as:
         num_obstacles = min + (difficulty / max_difficulty) * (max - min)
@@ -70,9 +70,9 @@ def reset_obstacles_with_individual_ranges(
     """
     obstacles: RigidObjectCollection = env.scene[asset_cfg.name]
 
-    num_objects = obstacles.num_objects
+    num_objects = obstacles.num_bodies
     num_envs = len(env_ids)
-    object_names = obstacles.object_names
+    object_names = obstacles.body_names
 
     # Get difficulty levels per environment
     if use_curriculum:
@@ -101,89 +101,84 @@ def reset_obstacles_with_individual_ranges(
     wall_names = list(wall_configs.keys())
     obstacle_types = list(obstacle_configs.values())
     env_size_t = torch.tensor(env_size, device=env.device)
-
-    # place walls
-    for wall_name, wall_cfg in wall_configs.items():
-        if wall_name in object_names:
-            wall_idx = object_names.index(wall_name)
-
-            min_ratio = torch.tensor(wall_cfg.center_ratio_min, device=env.device)
-            max_ratio = torch.tensor(wall_cfg.center_ratio_max, device=env.device)
-
-            if torch.allclose(min_ratio, max_ratio):
-                center_ratios = min_ratio.unsqueeze(0).repeat(num_envs, 1)
-            else:
-                ratios = torch.rand(num_envs, 3, device=env.device)
-                center_ratios = ratios * (max_ratio - min_ratio) + min_ratio
-
-            positions = (center_ratios - 0.5) * env_size_t
-            positions[:, 2] += ground_offset
-            positions += env.scene.env_origins[env_ids]
-
-            all_poses[:, wall_idx, 0:3] = positions
-            all_poses[:, wall_idx, 3:7] = torch.tensor([1.0, 0.0, 0.0, 0.0], device=env.device).repeat(num_envs, 1)
+    identity_quat = torch.tensor([0.0, 0.0, 0.0, 1.0], device=env.device)
+    all_poses[..., 3:7] = identity_quat
+
+    # Place walls
+    wall_entries = [(object_names.index(name), cfg) for name, cfg in wall_configs.items() if name in object_names]
+    if wall_entries:
+        wall_indices = [entry[0] for entry in wall_entries]
+        wall_min_ratios = torch.tensor(
+            [entry[1].center_ratio_min for entry in wall_entries], dtype=torch.float32, device=env.device
+        )
+        wall_max_ratios = torch.tensor(
+            [entry[1].center_ratio_max for entry in wall_entries], dtype=torch.float32, device=env.device
+        )
+        wall_center_ratios = wall_min_ratios.unsqueeze(0).expand(num_envs, -1, -1).clone()
+        variable_wall_indices = [
+            i
+            for i, (_, wall_cfg) in enumerate(wall_entries)
+            if wall_cfg.center_ratio_min != wall_cfg.center_ratio_max
+        ]
+        if variable_wall_indices:
+            wall_ratios = torch.rand(num_envs, len(variable_wall_indices), 3, device=env.device)
+            wall_center_ratios[:, variable_wall_indices] = (
+                wall_ratios
+                * (wall_max_ratios[variable_wall_indices] - wall_min_ratios[variable_wall_indices])
+                + wall_min_ratios[variable_wall_indices]
+            )
+        wall_positions = (wall_center_ratios - 0.5) * env_size_t
+        wall_positions[..., 2] += ground_offset
+        wall_positions += env.scene.env_origins[env_ids].unsqueeze(1)
+        all_poses[:, wall_indices, 0:3] = wall_positions
 
     # Get obstacle indices
     obstacle_indices = [idx for idx, name in enumerate(object_names) if name not in wall_names]
 
     if len(obstacle_indices) == 0:
-        obstacles.write_object_pose_to_sim(all_poses, env_ids=env_ids)
-        obstacles.write_object_velocity_to_sim(all_velocities, env_ids=env_ids)
+        obstacles.write_body_pose_to_sim_index(body_poses=all_poses, env_ids=env_ids)
+        obstacles.write_body_com_velocity_to_sim_index(body_velocities=all_velocities, env_ids=env_ids)
         return
 
-    # Determine which obstacles are active per env
-    active_masks = torch.zeros(num_envs, len(obstacle_indices), dtype=torch.bool, device=env.device)
-    for env_idx in range(num_envs):
-        num_active = obstacles_per_env[env_idx].item()
-        perm = torch.randperm(len(obstacle_indices), device=env.device)[:num_active]
-        active_masks[env_idx, perm] = True
-
-    # place obstacles
-    for obj_list_idx in range(len(obstacle_indices)):
-        obj_idx = obstacle_indices[obj_list_idx]
-
-        # Which envs need this obstacle?
-        envs_need_obstacle = active_masks[:, obj_list_idx]
-
-        if not envs_need_obstacle.any():
-            # Move all to -1000
-            all_poses[:, obj_idx, 0:3] = env.scene.env_origins[env_ids] + torch.tensor(
-                [0.0, 0.0, -1000.0], device=env.device
-            )
-            all_poses[:, obj_idx, 3:7] = torch.tensor([1.0, 0.0, 0.0, 0.0], device=env.device)
-            continue
-
-        # Get obstacle config
-        config_idx = obj_list_idx % len(obstacle_types)
-        obs_cfg = obstacle_types[config_idx]
-
-        min_ratio = torch.tensor(obs_cfg.center_ratio_min, device=env.device)
-        max_ratio = torch.tensor(obs_cfg.center_ratio_max, device=env.device)
-
-        # sample object positions
-        num_active_envs = envs_need_obstacle.sum().item()
-        ratios = torch.rand(num_active_envs, 3, device=env.device)
-        positions = (ratios * (max_ratio - min_ratio) + min_ratio - 0.5) * env_size_t
-        positions[:, 2] += ground_offset
-
-        # Add env origins
-        active_env_indices = torch.where(envs_need_obstacle)[0]
-        positions += env.scene.env_origins[env_ids[active_env_indices]]
-
-        # Generate quaternions
-        quats = math_utils.random_orientation(num_envs, device=env.device)
-
-        # Write poses
-        all_poses[envs_need_obstacle, obj_idx, 0:3] = positions
-        all_poses[envs_need_obstacle, obj_idx, 3:7] = quats[envs_need_obstacle]
-
-        # Move inactive obstacles far away
-        inactive = ~envs_need_obstacle
-        all_poses[inactive, obj_idx, 0:3] = env.scene.env_origins[env_ids[inactive]] + torch.tensor(
-            [0.0, 0.0, -1000.0], device=env.device
-        )
-        all_poses[inactive, obj_idx, 3:7] = torch.tensor([1.0, 0.0, 0.0, 0.0], device=env.device)
+    num_obstacles = len(obstacle_indices)
+
+    # Select the requested number of unique obstacles per environment
+    random_order = torch.argsort(torch.rand(num_envs, num_obstacles, device=env.device), dim=1)
+    active_by_rank = torch.arange(num_obstacles, device=env.device).unsqueeze(0) < obstacles_per_env.unsqueeze(1)
+    active_masks = torch.zeros(num_envs, num_obstacles, dtype=torch.bool, device=env.device)
+    active_masks.scatter_(1, random_order, active_by_rank)
+
+    # Sample every obstacle in one operation.
+    obstacle_min_ratios = torch.tensor(
+        [obstacle_types[i % len(obstacle_types)].center_ratio_min for i in range(num_obstacles)],
+        dtype=torch.float32,
+        device=env.device,
+    )
+    obstacle_max_ratios = torch.tensor(
+        [obstacle_types[i % len(obstacle_types)].center_ratio_max for i in range(num_obstacles)],
+        dtype=torch.float32,
+        device=env.device,
+    )
+    obstacle_ratios = torch.rand(num_envs, num_obstacles, 3, device=env.device)
+    obstacle_positions = (
+        obstacle_ratios * (obstacle_max_ratios - obstacle_min_ratios) + obstacle_min_ratios - 0.5
+    ) * env_size_t
+    obstacle_positions[..., 2] += ground_offset
+    obstacle_positions += env.scene.env_origins[env_ids].unsqueeze(1)
+
+    # Inactive samples are discarded
+    inactive_positions = env.scene.env_origins[env_ids].unsqueeze(1).expand(-1, num_obstacles, -1).clone()
+    inactive_positions[..., 2] += -1000.0 - 5.0 * torch.arange(num_obstacles, device=env.device)
+    obstacle_positions = torch.where(active_masks.unsqueeze(-1), obstacle_positions, inactive_positions)
+
+    obstacle_quats = math_utils.random_orientation(num_envs * num_obstacles, device=env.device).view(
+        num_envs, num_obstacles, 4
+    )
+    obstacle_quats = torch.where(active_masks.unsqueeze(-1), obstacle_quats, identity_quat)
+
+    all_poses[:, obstacle_indices, 0:3] = obstacle_positions
+    all_poses[:, obstacle_indices, 3:7] = obstacle_quats
 
     # Write to sim
-    obstacles.write_object_pose_to_sim(all_poses, env_ids=env_ids)
-    obstacles.write_object_velocity_to_sim(all_velocities, env_ids=env_ids)
+    obstacles.write_body_pose_to_sim_index(body_poses=all_poses, env_ids=env_ids)
+    obstacles.write_body_com_velocity_to_sim_index(body_velocities=all_velocities, env_ids=env_ids)

source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/navigation_env_cfg.py

@@ -7,6 +7,12 @@
 import math
 from dataclasses import MISSING
 
+from isaaclab_newton.physics import (
+    MJWarpSolverCfg,
+    NewtonCfg,
+    NewtonCollisionPipelineCfg,
+    NewtonShapeCfg,
+)
 from isaaclab_physx.physics import PhysxCfg
 
 import isaaclab.sim as sim_utils
@@ -44,6 +50,7 @@
     distance_to_goal_exp_curriculum,
     velocity_to_goal_reward_curriculum,
 )
+from isaaclab_tasks.utils import PresetCfg
 
 logging.getLogger("isaaclab.sensors.ray_caster.multi_mesh_ray_caster").setLevel(logging.WARNING)
 
@@ -56,6 +63,81 @@
 )
 
 
+##
+# Physics presets
+##
+
+
+@configclass
+class NavigationPhysicsCfg(PresetCfg):
+    """Physics backends supported by the ARL navigation task."""
+
+    default = PhysxCfg(gpu_max_rigid_patch_count=2**21)
+    newton_mjwarp = NewtonCfg(
+        solver_cfg=MJWarpSolverCfg(
+            njmax=768,
+            nconmax=128,
+            cone="elliptic",
+            impratio=100,
+            integrator="implicitfast",
+            use_mujoco_contacts=False,
+        ),
+        collision_cfg=NewtonCollisionPipelineCfg(max_triangle_pairs=2_500_000),
+        default_shape_cfg=NewtonShapeCfg(margin=0.01),
+    )
+    physx = default
+
+
+_DEPTH_CAMERA_CFG = MultiMeshRayCasterCameraCfg(
+    prim_path="{ENV_REGEX_NS}/Robot/base_link",
+    mesh_prim_paths=[
+        MultiMeshRayCasterCameraCfg.RaycastTargetCfg(
+            prim_expr=f"{{ENV_REGEX_NS}}/Obstacles/obstacle_{wall_name}", is_shared=False, track_mesh_transforms=True
+        )
+        for wall_name, _ in OBSTACLE_SCENE_CFG.wall_cfgs.items()
+    ]
+    + [
+        MultiMeshRayCasterCameraCfg.RaycastTargetCfg(
+            prim_expr=f"{{ENV_REGEX_NS}}/Obstacles/obstacle_{i}", is_shared=False, track_mesh_transforms=True
+        )
+        for i in range(OBSTACLE_SCENE_CFG.max_num_obstacles)
+    ],
+    offset=MultiMeshRayCasterCameraCfg.OffsetCfg(
+        pos=(0.15, 0.0, 0.04), rot=(1.0, 0.0, 0.0, 0.0), convention="world"
+    ),
+    update_period=0.1,
+    pattern_cfg=PinholeCameraPatternCfg(
+        width=480, height=270, focal_length=0.193, horizontal_aperture=0.36, vertical_aperture=0.21
+    ),
+    data_types=["distance_to_image_plane"],
+    max_distance=10.0,
+    depth_clipping_behavior="max",
+)
+
+
+@configclass
+class NavigationDepthCameraCfg(PresetCfg):
+    """Depth-camera implementations supported by the ARL navigation task."""
+
+    default = _DEPTH_CAMERA_CFG
+    newton_mjwarp = _DEPTH_CAMERA_CFG.replace(
+        class_type=(
+            "isaaclab_tasks.manager_based.drone_arl.navigation.config.arl_robot_1.newton_camera:"
+            "ArlNewtonMultiMeshRayCasterCamera"
+        )
+    )
+    physx = default
+
+
+@configclass
+class NavigationObstacleCollectionCfg(PresetCfg):
+    """Backend representations of the shared navigation obstacle scene."""
+
+    default = generate_obstacle_collection(OBSTACLE_SCENE_CFG)
+    newton_mjwarp = generate_obstacle_collection(OBSTACLE_SCENE_CFG, kinematic=True)
+    physx = default
+
+
 ##
 # Scene definition
 ##
@@ -64,37 +146,13 @@ class ArlNavigationSceneCfg(InteractiveSceneCfg):
     """Scene configuration for drone navigation with obstacles."""
 
     # obstacles
-    object_collection = generate_obstacle_collection(OBSTACLE_SCENE_CFG)
+    object_collection = NavigationObstacleCollectionCfg()
 
     # robots
     robot: MultirotorCfg = MISSING
 
     # sensors
-    depth_camera = MultiMeshRayCasterCameraCfg(
-        prim_path="{ENV_REGEX_NS}/Robot/base_link",
-        mesh_prim_paths=[
-            MultiMeshRayCasterCameraCfg.RaycastTargetCfg(
-                prim_expr=f"{{ENV_REGEX_NS}}/obstacle_{wall_name}", is_shared=False, track_mesh_transforms=True
-            )
-            for wall_name, _ in OBSTACLE_SCENE_CFG.wall_cfgs.items()
-        ]
-        + [
-            MultiMeshRayCasterCameraCfg.RaycastTargetCfg(
-                prim_expr=f"{{ENV_REGEX_NS}}/obstacle_{i}", is_shared=False, track_mesh_transforms=True
-            )
-            for i in range(OBSTACLE_SCENE_CFG.max_num_obstacles)
-        ],
-        offset=MultiMeshRayCasterCameraCfg.OffsetCfg(
-            pos=(0.15, 0.0, 0.04), rot=(1.0, 0.0, 0.0, 0.0), convention="world"
-        ),
-        update_period=0.1,
-        pattern_cfg=PinholeCameraPatternCfg(
-            width=480, height=270, focal_length=0.193, horizontal_aperture=0.36, vertical_aperture=0.21
-        ),
-        data_types=["distance_to_image_plane"],
-        max_distance=10.0,
-        depth_clipping_behavior="max",
-    )
+    depth_camera = NavigationDepthCameraCfg()
 
     contact_forces = ContactSensorCfg(
         prim_path="{ENV_REGEX_NS}/Robot/.*",
@@ -328,6 +386,7 @@ def __post_init__(self):
         # general settings
         self.decimation = 10
         self.episode_length_s = 10.0
+
         # simulation settings
         self.sim.dt = 0.01
         self.sim.render_interval = self.decimation
@@ -337,7 +396,8 @@ def __post_init__(self):
             static_friction=1.0,
             dynamic_friction=1.0,
         )
-        self.sim.physics = PhysxCfg(gpu_max_rigid_patch_count=2**21)
+        self.sim.physics = NavigationPhysicsCfg()
+
         # update sensor update periods
         # we tick all the sensors based on the smallest update period (physics update period)
         if self.scene.contact_forces is not None: