diff --git a/README.md b/README.md index f804a7d98..3776702a2 100644 --- a/README.md +++ b/README.md @@ -24,3 +24,579 @@ The repository is organized as follows: * the _assets_ directory contains meshes and other resources used by the examples and tests; * the _docs_ directory contains the sources for the documentation; * the _tests_ directory contains tests for the Scenic tool. + +# Scenic to Isaac Sim / Isaac Lab Interface + +This repository contains an interface for running Scenic scenarios in NVIDIA +Isaac Sim and Isaac Lab. The direct Isaac Sim path supports both the Isaac Sim +5.1 Core APIs and the Isaac Sim 6.0 Core Experimental APIs. The Isaac Lab path +translates a Scenic sample into an Isaac Lab manager-based environment. + +The main example scenarios are under [`examples/isaacsim`](examples/isaacsim). + +## Compatibility + +| Workflow | Python | Isaac Sim | Isaac Lab | Scenic backend | +| --- | --- | --- | --- | --- | +| Direct Isaac Sim 5.1 | 3.11 | 5.1.0 | Not required | `core_51` | +| Direct Isaac Sim 5.1 experimental | 3.11 | 5.1.0 | Not required | `experimental_51` | +| Direct Isaac Sim 6.0 | 3.12 | 6.0 | Not required | `experimental_60` | +| Isaac Lab, recommended/tested path | 3.11 | 5.1.0 | 2.3.x | `lab` | +| Isaac Lab 3.0 beta (not yet supported) | 3.12 | 6.0 | 3.0 beta | `lab` | + +> [!WARNING] +> The Isaac Lab integration was developed and tested against the Isaac Lab +> 2.x API used with Isaac Sim 5.1. Isaac Lab 3.0 is a major architectural +> release for Isaac Sim 6.0. Cloning the latest Isaac Lab source may therefore +> break this interface even though the direct Isaac Sim 6.0 backend works. +> Pin Isaac Lab to `v2.3.2` for the known working Lab setup. Isaac Lab +> `v3.0.0-beta` has not been validated and may required a large interface migration. + +The release-specific NVIDIA instructions are: + +* [Isaac Sim 5.1 pip installation](https://docs.isaacsim.omniverse.nvidia.com/5.1.0/installation/install_python.html) +* [Isaac Sim 6.0 pip installation](https://docs.isaacsim.omniverse.nvidia.com/6.0.0/installation/install_python.html) +* [Isaac Lab 2.3.2 installation](https://isaac-sim.github.io/IsaacLab/v2.3.2/source/setup/installation/pip_installation.html) +* [Isaac Lab 3.0 beta installation](https://isaac-sim.github.io/IsaacLab/v3.0.0-beta/source/setup/installation/pip_installation.html) + +## Installation + +Isaac Sim and Isaac Lab have strict dependency requirements. Use a dedicated +environment and install Isaac packages before installing Scenic. The commands +below target Linux x86-64 with CUDA 12.8. + +### Prerequisites + +* Ubuntu 22.04 or 24.04 +* A supported NVIDIA GPU and a current production driver +* GLIBC 2.35 or newer for pip-based Isaac Sim installation +* `git`, `pip`, and Python 3.11 or 3.12 as required below + +Check the GLIBC version with: + +```bash +ldd --version +``` + +### Option A: Isaac Sim 5.1.0 + +Use this environment for `core_51`, `experimental_51`, and the recommended +Isaac Lab 2.3.x setup: + +```bash +python3.11 -m venv .venv +source .venv/bin/activate +python -m pip install --upgrade pip setuptools wheel + +python -m pip install "isaacsim[all,extscache]==5.1.0" \ + --extra-index-url https://pypi.nvidia.com + +python -m pip install -U torch==2.7.0 torchvision==0.22.0 \ + --index-url https://download.pytorch.org/whl/cu128 +``` + +### Option B: Isaac Sim 6.0 + +Use this environment for `experimental_60`. Make sure to install PyTorch with the correct CUDA version (this example uses CUDA 12): + +```bash +python3.12 -m venv .venv +source .venv/bin/activate +python -m pip install --upgrade pip setuptools wheel + +python -m pip install torch==2.11.0 \ + --index-url https://download.pytorch.org/whl/cu128 + +python -m pip install "isaacsim[all,extscache]==6.0.0" \ + --extra-index-url https://pypi.nvidia.com +``` + +### Install Isaac Lab from source + +Isaac Lab is only required for commands that use `--param isaacLab True` or +the Scenic RSL-RL scripts. + +For the recommended Isaac Sim 5.1 setup, clone and pin Isaac Lab 2.3.2: + +```bash +git clone --branch v2.3.2 --depth 1 \ + https://github.com/isaac-sim/IsaacLab.git +cd IsaacLab + +sudo apt install cmake build-essential +./isaaclab.sh -i rsl_rl +``` + +The Scenic terrain integration requires one small Isaac Lab 2.x source change. +In `source/isaaclab/isaaclab/utils/configclass.py`, find the `_validate` +function and change: + +```python +if key.startswith("__"): +``` + +to: + +```python +if key.startswith("__") or key == "class_type": +``` + +Verify the source installation from the Isaac Lab repository: + +```bash +./isaaclab.sh -p scripts/tutorials/00_sim/create_empty.py +``` + +The experimental Isaac Sim 6.0 setup with Isaac Lab 3.0 has not been validated with the Scenic interface, and will most likely not work with the current interface without modifications. + +### Install Scenic + +From the root of this repository, with the Isaac environment activated: + +```bash +python -m pip install -e . --no-deps +``` + +`--no-deps` prevents pip from replacing versions required by Isaac Sim or Isaac +Lab. Install any missing Scenic dependencies selectively if they are not +already present in the Isaac environment. + +The terrain optimization example uses VerifAI, GPyOpt, and GPy: + +```bash +python -m pip install --no-deps "verifai==2.2.0" +python -m pip install --no-deps "GPyOpt==1.2.6" +python -m pip install --no-deps "GPy==1.13.2" +python -m pip install --no-deps "paramz==0.9.6" +``` + +## Running Scenic with Isaac Sim + +Every Isaac scenario must select the Isaac world model: + +```scenic +model scenic.simulators.isaac.model +``` + +Run with the Isaac Sim 5.1 Core backend: + +```bash +scenic examples/isaacsim/robot/create3.scenic -S -b \ + --param isaacBackend core_51 +``` + +Run the same scenario with the Isaac Sim 6.0 Core Experimental backend: + +```bash +scenic examples/isaacsim/robot/create3.scenic -S -b \ + --param isaacBackend experimental_60 +``` + +Add `--param headless True` to launch without the GUI: + +```bash +scenic examples/isaacsim/robot/create3.scenic -S -b \ + --param isaacBackend experimental_60 \ + --param headless True +``` + +### Isaac interface parameters + +All of the following parameters can be declared in a Scenic file with +`param NAME = VALUE` or overridden on the command line with +`--param NAME VALUE`. + +| Parameter | Default | Applies to | Description | +| --- | --- | --- | --- | +| `environmentUSDPath` | `None` | Sim and Lab | Local USD file, URL, or Isaac asset path such as `Isaac/Environments/...`. The interface loads the USD and prepares the mesh metadata Scenic needs for spatial reasoning. | +| `headless` | `False` | Sim and Lab | Launch Isaac Sim without its graphical window. | +| `isaacBackend` | `experimental_60` | Direct Sim | Selects `core_51`, `experimental_51`, or `experimental_60`. Match this to the installed Isaac Sim release. | +| `isaacLab` | `False` | Lab | When enabled, delegates simulation to the Isaac Lab manager-based interface. | +| `labTask` | `None` | Lab | Registered Isaac Lab task ID, for example `Isaac-Velocity-Rough-H1-v0`. The task must use a manager-based configuration, not a Direct environment. | +| `labEnvCfg` | `None` | Lab | Import string for a custom config class, for example `my_package.env_cfg:MyEnvCfg`. It must produce a `ManagerBasedEnvCfg` or `ManagerBasedRLEnvCfg`. | +| `labDevice` | `cuda:0` | Lab | Isaac Lab simulation device, such as `cuda:0` or `cpu`. | +| `labNumEnvs` | `1` | Lab | Number of parallel Isaac Lab environments. | +| `labEnvSpacing` | `10.0` | Lab | Distance between cloned Isaac Lab environments. | +| `labTimestep` | `1 / 120` | Lab | Physics timestep in seconds. | +| `labDebugLifecycle` | `False` | Lab | Enables periodic lifecycle and controller diagnostics. | + + +### Loading an Isaac environment + +An environment can be selected inside a scenario: + +```scenic +param environmentUSDPath = "Isaac/Environments/Simple_Warehouse/warehouse.usd" + +model scenic.simulators.isaac.model +``` + +It can also be overridden from the command line: + +```bash +scenic examples/isaacsim/forklift/forklift.scenic -S -b \ + --param isaacBackend experimental_60 \ + --param environmentUSDPath \ + Isaac/Environments/Simple_Warehouse/warehouse.usd +``` + +Converted environment data is cached. Local USD conversions are placed near +the source USD; Isaac asset and URL conversions are cached under +`~/.cache/scenic/isaac/environments`. + +## Running Scenic with Isaac Lab + +The simplest Lab run uses the built-in empty manager-based environment and +fills it with the sampled Scenic objects: + +```bash +scenic examples/isaacsim/robot/create3.scenic -S -b \ + --param isaacLab True \ + --param labNumEnvs 32 +``` + +To add the Scenic scene to a registered Isaac Lab manager-based task: + +```bash +scenic examples/isaacsim/robot/create3.scenic -S -b \ + --param isaacLab True \ + --param labTask Isaac-Velocity-Rough-H1-v0 \ + --param labNumEnvs 32 +``` + +To use a custom manager-based configuration instead of a registered task: + +```bash +scenic path/to/scenario.scenic -S -b \ + --param isaacLab True \ + --param labEnvCfg my_package.env_cfg:MyManagerBasedEnvCfg \ + --param labNumEnvs 32 +``` + +Specify either `labTask` or `labEnvCfg`. If neither is provided, the interface +uses its minimal empty manager-based configuration. + +> [!NOTE] +> The current Lab path samples one Scenic scene and lets Isaac Lab clone that +> configuration across `labNumEnvs`. The parallel environments therefore +> begin with the same Scenic object poses and terrain. Independent Scenic +> sampling per Lab environment is future work. + +### Scenic RSL-RL scripts + +The interface includes scripts for training and playing RSL-RL policies on +Scenic-generated terrain: + +```text +src/scenic/simulators/isaac/scripts/train_scenic.py +src/scenic/simulators/isaac/scripts/play_scenic.py +``` + +Important shared arguments are: + +| Argument | Description | +| --- | --- | +| `--scenario PATH` | Scenic terrain scenario. Defaults to `examples/isaacsim/terrain/random_uniform.scenic`. | +| `--scenic_model MODULE` | Scenic world model. Defaults to `scenic.simulators.isaac.model`. | +| `--terrain_border_width FLOAT` | Border added around the combined Scenic terrain mesh. | +| `--task TASK_ID` | Isaac Lab task to train or play. | +| `--agent ENTRY_POINT` | Agent configuration entry point. | +| `--num_envs N` | Number of parallel environments. | +| `--seed N` | Training or playback seed. | +| `--device DEVICE` | Isaac Lab device supplied by `AppLauncher`. | +| `--headless` | Run without the Isaac Sim GUI. | +| `--video` | Record a video and enable cameras. | +| `--video_length N` | Number of recorded steps. | +| `--checkpoint VALUE` | Checkpoint filename/pattern for training resume, or checkpoint path for playback. | +| `--experiment_name NAME` | Override the RSL-RL experiment name. | +| `--run_name NAME` | Add a name to the generated training run. | +| `--resume` | Resume training from a checkpoint. | +| `--load_run VALUE` | Run directory or run-selection pattern used when resuming. | +| `--logger LOGGER` | Select the external logger. | +| `--log_project_name NAME` | Project name for WandB or Neptune. | + +Training additionally supports `--max_iterations`, `--distributed`, +`--video_interval`, and `--export_io_descriptors`. Playback additionally +supports `--use_pretrained_checkpoint`, `--disable_fabric`, and `--real-time`. +The scripts also inherit release-specific `AppLauncher` and Hydra options from +Isaac Lab; run either script with `--help` for the complete list installed in +your pinned Isaac Lab version. + +## Writing a Scenic Scenario + +A minimal Isaac scenario can use ordinary Scenic geometry: + +```scenic +model scenic.simulators.isaac.model + +class Crate(IsaacSimObject): + shape: BoxShape() + width: 0.5 + length: 0.5 + height: 0.5 + density: 500 + color: (0.8, 0.2, 0.1) + +floor = new GroundPlane at (0, 0, -0.005), + with width 10, with length 10 + +crate = new Crate on floor + +terminate after 10 seconds +``` + +`IsaacSimObject` supports Scenic shapes as well as USD assets. Common +attributes are: + +| Attribute | Description | +| --- | --- | +| `width`, `length`, `height` | Scenic dimensions. Generic object USD assets are scaled to these dimensions. | +| `shape` | Scenic geometry used for placement and collision reasoning. | +| `physics` | Whether the object participates in physics. Defaults to `True`. | +| `mass`, `density` | Optional physical properties. | +| `color` | Display color. | +| `usd_path` | Local USD asset path. | +| `isaac_asset_path` | Isaac content path such as `Isaac/Robots/...`. | +| `initial_rotation` | Fixed rotation used to align the USD asset with Scenic coordinates. | + +When a scenario loads an existing environment, use `getExistingObj` to refer +to prims already in that USD stage: + +```scenic +param environmentUSDPath = "Isaac/Environments/Simple_Warehouse/warehouse.usd" + +model scenic.simulators.isaac.model +from scenic.simulators.isaac.utils import getExistingObj + +floor = getExistingObj("/Root/SM_floor58/SM_floor02") +robot = new Create3 on floor, with behavior KeepMoving + +terminate after 20 seconds +``` + +See [`examples/isaacsim/forklift/forklift.scenic`](examples/isaacsim/forklift/forklift.scenic) +for a complete environment-based scenario. + +### Scenic terrain for Isaac Lab + +The world model provides the following terrain classes: + +* `RandomUniformTerrain` +* `SlopedTerrain` +* `PyramidSlopedTerrain` +* `DiscreteObstaclesTerrain` +* `WaveTerrain` +* `StairsTerrain` +* `PyramidStairsTerrain` +* `SteppingStonesTerrain` +* `PolesTerrain` + +Multiple terrain objects can be placed relative to one another. The Lab +interface combines them into one terrain mesh and supplies the corresponding +environment origins to the Isaac Lab terrain importer. See +[`examples/isaacsim/terrain/random_uniform.scenic`](examples/isaacsim/terrain/random_uniform.scenic). + +## Adding a Custom Robot + +### Wheeled robot + +For a differential-drive robot, provide its USD asset and wheel metadata: + +```scenic +model scenic.simulators.isaac.model + +class MyDifferentialRobot(IsaacSimRobot): + width: 0.4 + length: 0.5 + height: 0.25 + usd_path: localPath("assets/my_robot.usd") + + wheel_controller: "differential" + wheel_radius: 0.06 + wheel_base: 0.32 + wheel_dof_names: ["left_wheel_joint", "right_wheel_joint"] + +behavior Drive(): + while True: + take applyController([0.5, 0.0]) + +floor = new GroundPlane +ego = new MyDifferentialRobot on floor, with behavior Drive +``` + +The differential command is `[linear_velocity, angular_velocity]`. The same +robot class can be translated into an Isaac Lab `ArticulationCfg`; the Lab +path currently provides built-in command handling for differential drive. + +The direct Isaac Sim backends also support: + +* Holonomic robots with `wheel_controller: "holonomic"` and a command of + `[x_velocity, y_velocity, yaw_velocity]`. +* Ackermann robots with `wheel_controller: "ackermann"`, drive and steering + DOF names, wheel base, track width, and wheel radius. The controller command + is `[steering_angle, steering_angle_velocity, forward_speed, acceleration, dt]`. + +The forklift example demonstrates an Ackermann robot with an additional lift +joint: +[`examples/isaacsim/forklift/common.scenic`](examples/isaacsim/forklift/common.scenic). + +### Custom articulation control + +For a robot that does not fit a built-in wheeled controller, provide a +`control` function that maps the Scenic behavior command to joint targets: + +```scenic +model scenic.simulators.isaac.model +from scenic.simulators.isaac.backends import articulation_action + +def velocityControl(command): + return articulation_action( + joint_velocities=command, + joint_velocity_indices=[0, 1] + ) + +class MyRobot(IsaacSimRobot): + width: 0.5 + length: 0.5 + height: 0.5 + usd_path: localPath("assets/my_robot.usd") + control: velocityControl + +behavior MoveJoints(): + while True: + take applyController([1.0, -1.0]) +``` + +An articulation action can provide `joint_positions`, `joint_velocities`, or +`joint_efforts`, together with the corresponding joint index field. This +controller form is shared by the direct Isaac Sim and Isaac Lab backends. + +For a custom Isaac Lab task, define and register a normal manager-based Isaac +Lab configuration, then select it with `labTask` or expose its config class +through `labEnvCfg`. Scenic adds sampled objects to `cfg.scene`; if a Scenic +object name matches an existing scene field, the interface updates that +asset's initial pose instead of adding a duplicate asset. + +## G1 Rough Locomotion Example + +This example workflow trains a Unitree G1 rough-terrain policy in Isaac Lab, evaluates +it on Scenic terrain, optionally continues training on Scenic terrain, and +then evaluates the updated checkpoint. + +### 1. Train a baseline G1 policy + +From the Isaac Lab repository: + +```bash +./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/train.py \ + --task Isaac-Velocity-Rough-G1-v0 \ + --num_envs 4096 \ + --headless +``` + +Isaac Lab writes the checkpoint under a directory such as: + +```text +IsaacLab/logs/rsl_rl/g1_rough//model_.pt +``` + +Reduce `--num_envs` if the GPU does not have enough memory. + +### 2. Place the checkpoint in the Scenic log tree + +From the Scenic repository: + +```bash +mkdir -p logs/rsl_rl/g1_rough/baseline +cp /path/to/model_2999.pt logs/rsl_rl/g1_rough/baseline/ +``` + +### 3. Play the baseline policy on Scenic terrain + +```bash +python src/scenic/simulators/isaac/scripts/play_scenic.py \ + --task Isaac-Velocity-Rough-G1-Play-v0 \ + --checkpoint logs/rsl_rl/g1_rough/baseline/model_2999.pt \ + --num_envs 32 \ + --scenario examples/isaacsim/terrain/random_uniform.scenic +``` + +The scenario combines randomized uniform noise, a pyramid slope, discrete +obstacles, and pyramid stairs. The G1 task keeps its robot, observations, +commands, rewards, and policy interface while its normal terrain generator is +replaced with the sampled Scenic terrain. + +### 4. Continue training on Scenic terrain + +```bash +python src/scenic/simulators/isaac/scripts/train_scenic.py \ + --task Isaac-Velocity-Rough-G1-v0 \ + --num_envs 4096 \ + --scenario examples/isaacsim/terrain/random_uniform.scenic \ + --resume \ + --load_run baseline \ + --checkpoint model_2999.pt \ + --headless +``` + +The continued run is written under: + +```text +logs/rsl_rl/g1_rough// +``` + +### 5. Evaluate the continued checkpoint + +```bash +python src/scenic/simulators/isaac/scripts/play_scenic.py \ + --task Isaac-Velocity-Rough-G1-Play-v0 \ + --checkpoint logs/rsl_rl/g1_rough//model_5998.pt \ + --num_envs 32 \ + --scenario examples/isaacsim/terrain/random_uniform.scenic +``` + +Replace the checkpoint names and run directory with the files produced by +your runs. Add `--headless` for non-interactive playback or `--video` to +record the evaluation. + +## Troubleshooting + +If Isaac Sim or Isaac Lab stops working after installing Scenic-related +packages, first check whether pip replaced an Isaac dependency: + +```bash +python -m pip check +python -m pip freeze | grep -E \ + "isaacsim|isaaclab|torch|numpy|scipy|trimesh|antlr4|opencv" +``` + +For reference, here is the working Isaac Sim 5.1 environment used at the time of testing: + +| Package | Version | +| --- | --- | +| `isaacsim` | `5.1.0.0` | +| `torch` | `2.7.0+cu128` | +| `torchvision` | `0.22.0+cu128` | +| `numpy` | `1.26.0` | +| `scipy` | `1.15.3` | +| `trimesh` | `4.5.1` | +| `antlr4-python3-runtime` | `4.9.3` | +| `opencv-python` | `4.11.0.86` | +| `verifai` | `2.2.0` | +| `GPy` | `1.13.2` | +| `GPyOpt` | `1.2.6` | + +Do not apply these 5.1 pins to an Isaac Sim 6.0 environment. + +Other common checks: + +* Confirm that the selected `isaacBackend` matches the installed Isaac Sim + release. +* Confirm that an Isaac Lab task is manager-based; Direct task configs are not + accepted by the general Scenic-to-Lab simulator. +* Apply the `configclass.py` change above if Scenic terrain validation descends + into `class_type`. +* Expect the first Isaac Sim launch to download and cache extensions and + assets, which can take several minutes. +* Use `-b` with the Scenic CLI or `--help` with the Lab scripts when diagnosing + startup and argument issues. diff --git a/examples/isaacsim/basic.scenic b/examples/isaacsim/basic.scenic index 3d8a5bb91..80b296fd4 100644 --- a/examples/isaacsim/basic.scenic +++ b/examples/isaacsim/basic.scenic @@ -21,4 +21,4 @@ plane = new IsaacSimObject left of chair by 1, with physics False, with length 2, with height 1, - facing directly toward ego \ No newline at end of file + facing directly toward ego diff --git a/examples/isaacsim/forklift/common.scenic b/examples/isaacsim/forklift/common.scenic new file mode 100644 index 000000000..eea8cf45c --- /dev/null +++ b/examples/isaacsim/forklift/common.scenic @@ -0,0 +1,194 @@ +model scenic.simulators.isaac.model + +import math + +param forkliftAssetPath = "Isaac/Robots/IsaacSim/ForkliftB/forklift_b.usd" + +LOWERED_LIFT_POSITION = -0.03 +CARRY_LIFT_POSITION = 1.85 +RELEASE_LIFT_POSITION = 1.45 + +LOAD_PICKUP_DISTANCE = 2.2 +SHELF_PLACE_DISTANCE = 2.35 + +APPROACH_SPEED = -1.5 +SHELF_APPROACH_SPEED = -1.5 +BACK_AWAY_SPEED = 0.45 +TURN_SPEED = -0.95 + +MAX_STEERING = 0.55 +TURN_TOLERANCE = math.radians(4) + +SETTLE_STEPS = 30 +LIFT_STEPS = 90 +LOWER_STEPS = 90 +PLACE_STEPS = 70 +BACK_AWAY_STEPS = 360 + + +def wrapAngle(angle): + while angle > math.pi: + angle -= 2.0 * math.pi + while angle < -math.pi: + angle += 2.0 * math.pi + return angle + +def headingErrorTo(targetHeading, obj): + return wrapAngle(targetHeading - obj.yaw) + + +class PalletLoad(IsaacSimObject): + width: 1.75 + length: 1.0 + height: 0.26 + density: 90 + isaac_asset_path: "Isaac/Props/Pallet/pallet.usd" + + +class AckermannForkliftB(IsaacSimRobot): + width: 1.3 + length: 2.8 + height: 2.2 + isaac_asset_path: globalParameters.forkliftAssetPath + initial_rotation: (-90 deg, 0, 0) + wheel_controller: "ackermann" + + # ForkliftB geometry/controller metadata. + wheel_base: 1.65 + track_width: 0.82 + wheel_radius: 0.255 + front_wheel_radius: 0.255 + back_wheel_radius: 0.255 + + max_wheel_velocity: 8.0 + max_wheel_rotation_angle: 0.69813 + max_acceleration: 0.8 + max_steering_angle_velocity: 1.0 + + # ForkliftB is a single rear-drive / rear-steer vehicle. + wheel_dof_names: ["back_wheel_drive"] + steering_dof_names: ["back_wheel_swivel"] + lift_dof_names: ["lift_joint"] + + steering_sign: 1.0 + + def move(self, sim, command): + """ + command format: + [steering_angle, speed, lift_position] + """ + steering, speed, lift_position = command + + if not hasattr(self, "_forklift_cached_dofs"): + self._drive_dof_indices = sim.backend.articulation_dof_indices( + sim, self, self.wheel_dof_names + ) + self._steer_dof_indices = sim.backend.articulation_dof_indices( + sim, self, self.steering_dof_names + ) + self._lift_dof_indices = sim.backend.articulation_dof_indices( + sim, self, self.lift_dof_names + ) + self._forklift_cached_dofs = True + + if self.controller is None: + return + + ackermann_command = [ + self.steering_sign * steering, + 0.0, # steering angle velocity + speed, # desired linear speed + 0.0, # acceleration + 0.0, # dt + ] + + steering_positions, wheel_velocities = self.controller.forward(ackermann_command) + + # ForkliftB has one steering swivel and one drive joint. + steering_position = float(steering_positions[0]) + wheel_velocity = float(wheel_velocities[0]) + + action = sim.backend.articulation_action( + joint_positions=[steering_position, float(lift_position)], + joint_position_indices=( + self._steer_dof_indices + self._lift_dof_indices + ), + joint_velocities=[wheel_velocity], + joint_velocity_indices=self._drive_dof_indices, + ) + sim.backend.apply_articulation_action( + sim, + self, + action, + ) + +behavior HoldForks(liftPosition, steps=30): + for _ in range(steps): + take applyController([0.0, 0.0, liftPosition]) + + +behavior DriveStraight(speed, liftPosition, steps): + for _ in range(steps): + take applyController([0.0, speed, liftPosition]) + + +behavior DriveToObject(target, stopDistance, speed, liftPosition): + while (distance from ego to target) > stopDistance: + take applyController([0.0, speed, liftPosition]) + + take applyController([0.0, 0.0, liftPosition]) + + +behavior TurnToHeading(targetHeading, liftPosition): + while abs(headingErrorTo(targetHeading, ego)) > TURN_TOLERANCE: + error = headingErrorTo(targetHeading, ego) + steering = math.copysign(MAX_STEERING, error) + take applyController([steering, TURN_SPEED, liftPosition]) + + take applyController([0.0, 0.0, liftPosition]) + + +behavior LowerForks(): + do HoldForks(LOWERED_LIFT_POSITION, SETTLE_STEPS) + + +behavior LiftForks(): + do HoldForks(CARRY_LIFT_POSITION, LIFT_STEPS) + + +behavior ReleaseLoad(): + do HoldForks(RELEASE_LIFT_POSITION, LOWER_STEPS) + + +behavior BackAwayFromShelf(): + do DriveStraight(BACK_AWAY_SPEED, RELEASE_LIFT_POSITION, BACK_AWAY_STEPS) + + +behavior AckermannForkliftPickup(load, shelf): + # 1. Lower the forks. + do LowerForks() + + # 2. Drive straight into the pallet. + do DriveToObject(load, LOAD_PICKUP_DISTANCE, APPROACH_SPEED, LOWERED_LIFT_POSITION) + + # 3. Lift the pallet. + do LiftForks() + + # 4. Turn toward the shelf aisle. + do TurnToHeading(0 deg, CARRY_LIFT_POSITION) + + # 5. Drive toward the shelf. + do DriveToObject(shelf, SHELF_PLACE_DISTANCE, SHELF_APPROACH_SPEED, CARRY_LIFT_POSITION) + + # 6. Ease forward a little to place the pallet. + do DriveStraight(SHELF_APPROACH_SPEED * 0.5, CARRY_LIFT_POSITION, PLACE_STEPS) + + # 7. Lower/release the pallet. + do ReleaseLoad() + + # 8. Back away. + do BackAwayFromShelf() + + # 9. Stop. + while True: + take applyController([0.0, 0.0, RELEASE_LIFT_POSITION]) diff --git a/examples/isaacsim/forklift/forklift.scenic b/examples/isaacsim/forklift/forklift.scenic new file mode 100644 index 000000000..992c1ccfe --- /dev/null +++ b/examples/isaacsim/forklift/forklift.scenic @@ -0,0 +1,21 @@ +param environmentUSDPath = "Isaac/Environments/Simple_Warehouse/warehouse.usd" +param duration = 60 + +model scenic.simulators.isaac.model +from scenic.simulators.isaac.utils import getExistingObj +from common import * + +floor_piece = getExistingObj("/Root/SM_floor58/SM_floor02") +shelf = getExistingObj("/Root/SM_RackShelf_159/SM_RackShelf_01/Section1") +pallet = getExistingObj("/Root/SM_PaletteA_12/SM_PaletteA_01") + +approach_heading = Range(0, 180 deg) + +load = new PalletLoad on floor_piece, behind pallet by 4, + facing approach_heading + +ego = new AckermannForkliftB behind load by Range(2, 3), + facing approach_heading, + with behavior AckermannForkliftPickup(load, shelf) + +terminate after globalParameters.duration seconds diff --git a/examples/isaacsim/robot/franka.scenic b/examples/isaacsim/robot/franka.scenic index 728764463..d033b1f15 100644 --- a/examples/isaacsim/robot/franka.scenic +++ b/examples/isaacsim/robot/franka.scenic @@ -1,5 +1,5 @@ param environmentUSDPath = "Isaac/Environments/Simple_Room/simple_room.usd" -param duration = 10 +param duration = 30 param cubeSize = 0.0515 param binHeight = 0.1475 param dropHeight = 0.2 @@ -37,11 +37,8 @@ ego = new FrankaPanda on table, at (0, 0), Vector(small_bin.x, small_bin.y, small_bin.z + globalParameters.dropHeight), ) -print(f"CUBE POS: {cube.x}, {cube.y}, {cube.z}") -print(f"SMALL_BIN POS: {small_bin.x}, {small_bin.y}, {small_bin.z}") - require 0.42 <= distance from cube to ego <= 0.82 require 0.25 <= distance from small_bin to ego <= 0.82 require distance from cube to small_bin > 0.38 -terminate after globalParameters.duration * 2 seconds +terminate after globalParameters.duration seconds diff --git a/examples/isaacsim/robot/franka_panda_robot.scenic b/examples/isaacsim/robot/franka_panda_robot.scenic deleted file mode 100644 index 54b51a1af..000000000 --- a/examples/isaacsim/robot/franka_panda_robot.scenic +++ /dev/null @@ -1,253 +0,0 @@ -model scenic.simulators.isaac.model - -import numpy as np - -from scenic.simulators.isaac.backends import get_backend_version -import scenic.simulators.isaac.utils as scenic_utils - -IK_METHODS = { - "singular-value-decomposition", - "pseudoinverse", - "transpose", - "damped-least-squares", -} - -DEFAULT_DOF_POSITIONS = np.array( - [0.012, -0.568, 0.0, -2.811, 0.0, 3.037, 0.741, 0.0, 0.0], - dtype=float, -) -OPEN_GRIPPER_POSITIONS = [0.05, 0.05] -CLOSED_GRIPPER_POSITIONS = [0.005, 0.005] -DOWNWARD_ORIENTATION = np.array([0.0, 1.0, 0.0, 0.0], dtype=float) -ARM_DOF_INDICES = list(range(7)) -GRIPPER_DOF_INDICES = [7, 8] -PICK_PLACE_STATES = {} - - -def _quat_mul(a, b): - w1, x1, y1, z1 = a[:, 0], a[:, 1], a[:, 2], a[:, 3] - w2, x2, y2, z2 = b[:, 0], b[:, 1], b[:, 2], b[:, 3] - ww = (z1 + x1) * (x2 + y2) - yy = (w1 - y1) * (w2 + z2) - zz = (w1 + y1) * (w2 - z2) - xx = ww + yy + zz - qq = 0.5 * (xx + (z1 - x1) * (x2 - y2)) - return np.stack( - [ - qq - ww + (z1 - y1) * (y2 - z2), - qq - xx + (x1 + w1) * (x2 + w2), - qq - yy + (w1 - x1) * (y2 + z2), - qq - zz + (z1 + y1) * (w2 - x2), - ], - axis=-1, - ) - - -def _quat_conjugate(q): - return np.concatenate((q[:, :1], -q[:, 1:]), axis=-1) - - -def differential_inverse_kinematics( - jacobian, - current_position, - current_orientation, - goal_position, - goal_orientation=None, - method="damped-least-squares", - scale=1.0, - damping=0.05, - min_singular_value=1e-5, -): - jacobian = np.asarray(jacobian, dtype=float) - if jacobian.shape != (6, 7): - raise ValueError(f"expected a 6x7 end-effector Jacobian, got {jacobian.shape}") - - current_position = np.asarray(current_position, dtype=float).reshape(1, 3) - current_orientation = np.asarray(current_orientation, dtype=float).reshape(1, 4) - goal_position = np.asarray(goal_position, dtype=float).reshape(1, 3) - goal_orientation = ( - current_orientation if goal_orientation is None else goal_orientation - ) - goal_orientation = np.asarray(goal_orientation, dtype=float).reshape(1, 4) - - q = _quat_mul(goal_orientation, _quat_conjugate(current_orientation)) - error = np.concatenate( - [goal_position - current_position, q[:, 1:] * np.sign(q[:, [0]])], - axis=-1, - ).reshape(6) - - scale = float(scale) - damping = float(damping) - min_singular_value = float(min_singular_value) - - if method == "singular-value-decomposition": - U, S, Vh = np.linalg.svd(jacobian, full_matrices=False) - inv_s = np.where(S > min_singular_value, 1.0 / S, 0.0) - jacobian_inverse = np.matmul(np.matmul(Vh.T, np.diag(inv_s)), U.T) - elif method == "pseudoinverse": - jacobian_inverse = np.linalg.pinv(jacobian) - elif method == "transpose": - jacobian_inverse = jacobian.T - elif method == "damped-least-squares": - lhs = np.matmul(jacobian, jacobian.T) + np.eye(6, dtype=float) * (damping**2) - jacobian_inverse = np.matmul( - jacobian.T, - np.linalg.solve(lhs, np.eye(6, dtype=float)), - ) - else: - raise ValueError(f"invalid IK method {method!r}") - - return scale * np.matmul(jacobian_inverse, error) - - -def _new_pick_place_state(): - return { - "initialized": False, - "stage": 0, - "stage_steps": 0, - "done": False, - "pick_position": None, - "place_position": None, - "end_effector": None, - "end_effector_link_index": None, - } - - -def _initialize_franka(sim, obj, state): - from isaacsim.core.experimental.prims import RigidPrim - - handle = sim.world.get_object(obj.name) - robot = handle.wrapper - state["end_effector"] = RigidPrim(f"{handle.prim_path}/panda_hand") - state["end_effector_link_index"] = robot.get_link_indices("panda_hand").list()[0] - robot.set_default_state(dof_positions=DEFAULT_DOF_POSITIONS.tolist()) - robot.reset_to_default_state() - state["initialized"] = True - return handle - - -def _move_arm(robot, state, target, method, scale, damping, min_singular_value): - current_position, current_orientation = state["end_effector"].get_world_poses() - current_position = current_position.numpy() - current_orientation = current_orientation.numpy() - - jacobians = robot.get_jacobian_matrices().numpy() - jacobian = jacobians[0, state["end_effector_link_index"] - 1, :, :7] - delta = differential_inverse_kinematics( - jacobian, - current_position, - current_orientation, - np.asarray(target, dtype=float).reshape(1, 3), - goal_orientation=np.array([DOWNWARD_ORIENTATION], dtype=float), - method=method, - scale=scale, - damping=damping, - min_singular_value=min_singular_value, - ) - - current_dof_positions = robot.get_dof_positions().numpy() - arm_targets = ( - current_dof_positions[:, ARM_DOF_INDICES] + delta - ).reshape(-1).tolist() - robot.set_dof_position_targets(arm_targets, dof_indices=ARM_DOF_INDICES) - - -def franka_pick_place_control(sim, obj, command): - target_object, goal_position, method, scale, damping, min_singular_value = command - scale = float(scale) - damping = float(damping) - min_singular_value = float(min_singular_value) - - state_key = (id(sim), obj.name) - state = PICK_PLACE_STATES.get(state_key) - if state is None: - state = PICK_PLACE_STATES[state_key] = _new_pick_place_state() - if state["done"]: - return - - handle = ( - sim.world.get_object(obj.name) - if state["initialized"] - else _initialize_franka(sim, obj, state) - ) - robot = handle.wrapper - if state["pick_position"] is None: - target_handle = sim.world.get_object(target_object.name) - state["pick_position"] = ( - target_handle.wrapper.get_world_poses()[0].numpy()[0].copy() - ) - if state["place_position"] is None: - state["place_position"] = scenic_utils.vectorToArray(goal_position).copy() - - phases = [ - ( - state["pick_position"] + np.array([0.0, 0.0, 0.20]), - OPEN_GRIPPER_POSITIONS, - 120, - ), - ( - state["pick_position"] + np.array([0.0, 0.0, 0.10]), - OPEN_GRIPPER_POSITIONS, - 80, - ), - (None, CLOSED_GRIPPER_POSITIONS, 100), - ( - state["pick_position"] + np.array([0.0, 0.0, 0.50]), - CLOSED_GRIPPER_POSITIONS, - 150, - ), - ( - state["place_position"] + np.array([0.0, 0.0, 0.50]), - CLOSED_GRIPPER_POSITIONS, - 180, - ), - ( - state["place_position"] + np.array([0.0, 0.0, 0.20]), - CLOSED_GRIPPER_POSITIONS, - 90, - ), - (None, OPEN_GRIPPER_POSITIONS, 20), - ] - target, gripper_positions, steps = phases[state["stage"]] - if target is not None: - _move_arm(robot, state, target, method, scale, damping, min_singular_value) - robot.set_dof_position_targets( - list(gripper_positions), dof_indices=GRIPPER_DOF_INDICES - ) - state["stage_steps"] += 1 - if state["stage_steps"] > steps: - print(f"CustomFranka stage={state['stage']}, method={method}, target={target}") - state["stage"] += 1 - state["stage_steps"] = 0 - state["done"] = state["stage"] >= len(phases) - - -behavior FrankaPickPlaceWithIK( - target_object, - goal_position, - ik_method, - ik_scale, - ik_damping, - ik_min_singular_value, -): - while True: - take applyController( - [ - target_object, - goal_position, - ik_method, - ik_scale, - ik_damping, - ik_min_singular_value, - ] - ) - - -class CustomFrankaPanda(IsaacSimRobot): - width: 0.3 - length: 0.3 - height: 0.01 - isaac_asset_path: "Isaac/Robots/FrankaRobotics/FrankaPanda/franka.usd" - - def move(self, sim, command): - franka_pick_place_control(sim, self, command) diff --git a/examples/isaacsim/robot/franka_pick_place_custom_ik.scenic b/examples/isaacsim/robot/franka_pick_place_custom_ik.scenic deleted file mode 100644 index 7b6756d33..000000000 --- a/examples/isaacsim/robot/franka_pick_place_custom_ik.scenic +++ /dev/null @@ -1,71 +0,0 @@ -param isaacBackend = "experimental_60" -param environmentUSDPath = "Isaac/Environments/Simple_Room/simple_room.usd" -param duration = 10 -param cubeSize = 0.0515 -param binHeight = 0.1475 -param dropHeight = 0.2 - -param ikMethod = "damped-least-squares" -param ikScale = 1.0 -param ikDamping = 0.05 -param ikMinSingularValue = 1e-5 - -model scenic.simulators.isaac.model -from scenic.simulators.isaac.utils import getExistingObj -from franka_panda_robot import * - -ik_method = globalParameters.ikMethod -ik_scale = globalParameters.ikScale -ik_damping = globalParameters.ikDamping -ik_min_singular_value = globalParameters.ikMinSingularValue -drop_height = globalParameters.dropHeight -duration = globalParameters.duration - -if ik_method not in IK_METHODS: - raise ValueError( - f"invalid ikMethod {ik_method!r}; expected one of {sorted(IK_METHODS)}" - ) - -table = getExistingObj("/Root/table_low_327/table_low") - -class IsaacBin(IsaacSimObject): - length: 0.3 - width: 0.2 - height: globalParameters.binHeight - physics: False - shape: BoxShape() - isaac_asset_path: "Isaac/Props/KLT_Bin/small_KLT.usd" - -class PickCube(IsaacSimObject): - width: globalParameters.cubeSize - length: globalParameters.cubeSize - height: globalParameters.cubeSize - mass: 0.05 - color: (1, 0, 0) - shape: BoxShape() - -cube = new PickCube on table - -small_bin = new IsaacBin on table, - facing toward cube - -ego = new CustomFrankaPanda on table, at (0, 0), - facing toward cube, - with behavior FrankaPickPlaceWithIK( - cube, - Vector(small_bin.x, small_bin.y, small_bin.z + drop_height), - ik_method, - ik_scale, - ik_damping, - ik_min_singular_value, - ) - -print(f"CUBE POS: {cube.x}, {cube.y}, {cube.z}") -print(f"SMALL_BIN POS: {small_bin.x}, {small_bin.y}, {small_bin.z}") -print(f"IK METHOD: {ik_method}") - -require 0.42 <= distance from cube to ego <= 0.82 -require 0.25 <= distance from small_bin to ego <= 0.82 -require distance from cube to small_bin > 0.38 - -terminate after duration * 2 seconds diff --git a/examples/isaacsim/robot/jetbot_built_in.scenic b/examples/isaacsim/robot/jetbot_built_in.scenic new file mode 100644 index 000000000..617078bc9 --- /dev/null +++ b/examples/isaacsim/robot/jetbot_built_in.scenic @@ -0,0 +1,17 @@ +model scenic.simulators.isaac.model + +floor = new GroundPlane with color (1, 1, 1), with width 8, with length 8 +floor_region = RectangularRegion(0 @ 0, 0, 8.1, 8.1) +workspace = Workspace(floor_region) + +class Cube(IsaacSimObject): + shape: BoxShape() + width: 0.2 + length: 0.2 + height: 0.2 + isaac_asset_path: "Isaac/Props/Rubiks_Cube/rubiks_cube.usd" + +new Jetbot on floor, with behavior JetbotDrive + +for _ in range(25): + new Cube on floor, facing Range(0, 360 deg) diff --git a/examples/isaacsim/robot/kaya2.scenic b/examples/isaacsim/robot/kaya2.scenic new file mode 100644 index 000000000..81588ba93 --- /dev/null +++ b/examples/isaacsim/robot/kaya2.scenic @@ -0,0 +1,24 @@ +model scenic.simulators.isaac.model + +class Box(IsaacSimObject): + width: .2 + height: .2 + length: .2 + density: 1 + color: Uniform([1, 0.502, 0], [1, 0, 0], [0, 1, 1], [1, 0, 1]) + +floor = new GroundPlane with color (1, 1, 1), with width 5, with length 5 + +room_region = RectangularRegion(0 @ 0, 0, 5.09, 5.09) +workspace = Workspace(room_region) + +b1 = new Box on floor +ego = new Kaya on floor, facing toward b1, with behavior DriveForward, ahead of b1 by 2 + +b2 = new Box right of b1 by .01 +b3 = new Box left of b1 by .01 +b4 = new Box on b1 +b5 = new Box on b4 +b6 = new Box on b2 +b7 = new Box on b3 +b8 = new Box on b5 \ No newline at end of file diff --git a/examples/isaacsim/robot/kaya_random.scenic b/examples/isaacsim/robot/kaya_random.scenic index 359173dc7..59d29df51 100644 --- a/examples/isaacsim/robot/kaya_random.scenic +++ b/examples/isaacsim/robot/kaya_random.scenic @@ -4,7 +4,7 @@ class Box(IsaacSimObject): width: .2 height: .2 length: .2 - density: 50 + density: 1 color: Uniform([1, 0.502, 0], [1, 0, 0], [0, 1, 1], [1, 0, 1]) floor = new GroundPlane with color (1, 1, 1), with width 5, with length 5 diff --git a/examples/isaacsim/simple_room_big_existing.scenic b/examples/isaacsim/simple_room_big_existing.scenic index 8e2032344..97a34b02a 100644 --- a/examples/isaacsim/simple_room_big_existing.scenic +++ b/examples/isaacsim/simple_room_big_existing.scenic @@ -24,8 +24,5 @@ class IsaacAssetBox(IsaacSimObject): test_asset = new IsaacAssetBox on simple_room_table -print(f"TEST ASSET POS:") -print(test_asset.position) - reference_toy = new Toy on simple_room_table, right of test_asset by 0.35, with physics False diff --git a/examples/isaacsim/simple_room_existing2.scenic b/examples/isaacsim/simple_room_existing2.scenic index 921d33d04..f3f7b42d3 100644 --- a/examples/isaacsim/simple_room_existing2.scenic +++ b/examples/isaacsim/simple_room_existing2.scenic @@ -24,8 +24,5 @@ class IsaacAssetBox(IsaacSimObject): test_asset = new IsaacAssetBox on simple_room_table -print(f"TEST ASSET POS:") -print(test_asset.position) - reference_toy = new Toy on simple_room_table, right of test_asset by 0.35, with physics False diff --git a/examples/isaacsim/simple_room_isaac_browser.scenic b/examples/isaacsim/simple_room_isaac_browser.scenic index 4e782a391..4c462872f 100644 --- a/examples/isaacsim/simple_room_isaac_browser.scenic +++ b/examples/isaacsim/simple_room_isaac_browser.scenic @@ -24,8 +24,5 @@ class IsaacAssetBox(IsaacSimObject): test_asset = new IsaacAssetBox on simple_room_table -print(f"TEST ASSET POS:") -print(test_asset.position) - reference_toy = new Toy right of test_asset by 0.35, with physics False diff --git a/examples/isaacsim/simple_room_regular_existing.scenic b/examples/isaacsim/simple_room_regular_existing.scenic index 159452959..4db351ac3 100644 --- a/examples/isaacsim/simple_room_regular_existing.scenic +++ b/examples/isaacsim/simple_room_regular_existing.scenic @@ -17,8 +17,5 @@ class IsaacAssetBox(IsaacSimObject): test_asset = new IsaacAssetBox on simple_room_table -print(f"TEST ASSET POS:") -print(test_asset.position) - reference_toy = new Toy on simple_room_table, right of test_asset by 0.35, with physics False diff --git a/examples/isaacsim/terrain/random_uniform.scenic b/examples/isaacsim/terrain/random_uniform.scenic new file mode 100644 index 000000000..e13174aa1 --- /dev/null +++ b/examples/isaacsim/terrain/random_uniform.scenic @@ -0,0 +1,43 @@ +model scenic.simulators.isaac.model + +param isaacLab = True + +param verifaiSamplerType = "bo" + +param uniform_noise_upper = VerifaiRange(0.4, 1.5) +param uniform_noise_range = (VerifaiRange(0.1, 0.4), globalParameters.uniform_noise_upper) + +param discrete_obstacles_max_height = VerifaiRange(0.1, 0.7) +param discrete_obstacles_min_size = VerifaiRange(0.1, 0.3) +param discrete_obstacles_max_size = VerifaiRange(1.0, 2.0) +param discrete_obstacles_num_rects = VerifaiDiscreteRange(100, 150) +param discrete_obstacles_platform_size = VerifaiRange(3, 5) + +param pyramid_stairs_step_width = VerifaiRange(0.5, 1.5) +param pyramid_stairs_step_height = VerifaiRange(0.1, 0.4) + +param slope = VerifaiRange(0.3, 0.8) + +terrain = new RandomUniformTerrain at (0, 0, 0), + with width 20, + with length 20, + with noise_range globalParameters.uniform_noise_range + +pyramid_sloped_terrain = new PyramidSlopedTerrain right of terrain, + with width 20, with length 20, + with slope globalParameters.slope + +discrete_obstacles_terrain = new DiscreteObstaclesTerrain ahead of pyramid_sloped_terrain, + with width 20, + with length 20, + with max_height globalParameters.discrete_obstacles_max_height, + with min_size globalParameters.discrete_obstacles_min_size, + with max_size globalParameters.discrete_obstacles_max_size, + with num_rects globalParameters.discrete_obstacles_num_rects, + with platform_size globalParameters.discrete_obstacles_platform_size + +pyramid_stairs_terrain = new PyramidStairsTerrain left of discrete_obstacles_terrain, + with width 20, + with length 20, + with step_width globalParameters.pyramid_stairs_step_width, + with step_height globalParameters.pyramid_stairs_step_height diff --git a/examples/isaacsim/terrain/room.scenic b/examples/isaacsim/terrain/room.scenic new file mode 100644 index 000000000..66338a59c --- /dev/null +++ b/examples/isaacsim/terrain/room.scenic @@ -0,0 +1,34 @@ +model scenic.simulators.isaac.model + +import sys +sys.path.append(str(localPath(".."))) + +from lib import * +param uniform_noise_range = (Range(0.1, 0.2), Range(0.2, 0.5)) + +terrain = new RandomUniformTerrain at (0, 0, 0), + with width 20, + with length 20, + with noise_range globalParameters.uniform_noise_range + +sofa = new Couch on terrain + +coffee_table = new CoffeeTable on terrain, ahead of sofa by Range(0.5, 1.5) + +wall = new Wall on terrain, behind sofa by Range(0.1, 0.2) + +dining_room_region = RectangularRegion(1.25 @ 0, 0, 2.5, 5) + +dining_table = new DiningTable contained in dining_room_region, on terrain, + facing Range(0, 360 deg) + +chair_1 = new DiningChair behind dining_table by -0.1, on terrain, + facing toward dining_table, with regionContainedIn dining_room_region +chair_2 = new DiningChair ahead of dining_table by -0.1, on terrain, + facing toward dining_table, with regionContainedIn dining_room_region +chair_3 = new DiningChair left of dining_table by -0.1, on terrain, + facing toward dining_table, with regionContainedIn dining_room_region + +# Spawn some toys +for _ in range(20): + new Toy on terrain \ No newline at end of file diff --git a/examples/isaacsim/terrain/sloped_terrain.scenic b/examples/isaacsim/terrain/sloped_terrain.scenic new file mode 100644 index 000000000..fa66e3db3 --- /dev/null +++ b/examples/isaacsim/terrain/sloped_terrain.scenic @@ -0,0 +1,6 @@ +model scenic.simulators.isaac.model + +param numToys = 0 +param duration = 10 + +terrain = new SlopedTerrain with slope_angle Uniform(45 deg, 90 deg) \ No newline at end of file diff --git a/examples/isaacsim/terrain/test.scenic b/examples/isaacsim/terrain/test.scenic new file mode 100644 index 000000000..fbc9820ed --- /dev/null +++ b/examples/isaacsim/terrain/test.scenic @@ -0,0 +1,22 @@ +model scenic.simulators.isaac.model + +param isaacLab = True + +param uniform_noise_range = (VerifaiRange(0.1, 0.4), VerifaiRange(0.4, 1.5)) + +param discrete_obstacles_max_height = VerifaiRange(0.1, 0.7) +param discrete_obstacles_min_size = VerifaiRange(0.1, 0.3) +param discrete_obstacles_max_size = VerifaiRange(1.0, 2.0) +param discrete_obstacles_num_rects = VerifaiDiscreteRange(100, 150) +param discrete_obstacles_platform_size = VerifaiRange(3, 5) + +param pyramid_stairs_step_width = VerifaiRange(0.5, 1.5) +param pyramid_stairs_step_height = VerifaiRange(0.1, 0.4) + +param slope = VerifaiRange(0.3, 0.8) + +stairs_terrain = new PyramidStairsTerrain at (0, 0, 0), + with width 20, + with length 20, + with step_width 0.2, + with step_height 0.1 diff --git a/examples/isaacsim/warehouse_with_forklifts.scenic b/examples/isaacsim/warehouse_with_forklifts.scenic new file mode 100644 index 000000000..18462374a --- /dev/null +++ b/examples/isaacsim/warehouse_with_forklifts.scenic @@ -0,0 +1,8 @@ +param environmentUSDPath = "Isaac/Environments/Simple_Warehouse/warehouse_with_forklifts.usd" + +model scenic.simulators.isaac.model +from scenic.simulators.isaac.utils import getExistingObj + +floor_piece = getExistingObj("/Root/SM_floor58/SM_floor02") + +new Create3 on floor_piece, with color (1, 0, 0), with behavior KeepMoving \ No newline at end of file diff --git a/src/scenic/simulators/isaac/__init__.py b/src/scenic/simulators/isaac/__init__.py index 16ad048ee..d2919eabd 100644 --- a/src/scenic/simulators/isaac/__init__.py +++ b/src/scenic/simulators/isaac/__init__.py @@ -1 +1,11 @@ -from .simulator import IsaacSimSimulator +from .simulator import IsaacSimulator, IsaacSimSimulator + +try: + from .lab import IsaacLabSimulator +except ModuleNotFoundError: + IsaacLabSimulator = None + + +class TerrainBase: + horizontal_scale: float = 0.1 + vertical_scale: float = 0.005 diff --git a/src/scenic/simulators/isaac/actions.py b/src/scenic/simulators/isaac/actions.py index dd23d5d97..41967ad36 100644 --- a/src/scenic/simulators/isaac/actions.py +++ b/src/scenic/simulators/isaac/actions.py @@ -13,18 +13,6 @@ def canBeTakenBy(self, agent): class RobotAction(Action): def canBeTakenBy(self, agent): return isinstance(agent, _Robot) - -class WheeledRobotAction(Action): - def canBeTakenBy(self, agent): - return isinstance(agent, _WheeledRobot) - -class HolonomicRobotAction(Action): - def canBeTakenBy(self, agent): - return isinstance(agent, _HolonomicRobot) - -class QuadrupedRobotAction(Action): - def canBeTakenBy(self, agent): - return isinstance(agent, _QuadrupedRobot) class applyController(RobotAction): @@ -34,25 +22,6 @@ def __init__(self, command): def applyTo(self, obj, sim): obj.move(sim, self.command) -class applyWheeledController(WheeledRobotAction): - - def __init__(self, throttle=0, steering=0): - self.throttle = throttle - self.steering = steering - - def applyTo(self, obj, sim): - obj.move(sim, self.throttle, self.steering) - -class applyHolonomicController(HolonomicRobotAction): - - def __init__(self, forward_speed=0, lateral_speed=0, yaw_speed=0): - self.forward_speed = forward_speed - self.lateral_speed = lateral_speed - self.yaw_speed = yaw_speed - - def applyTo(self, obj, sim): - obj.move(sim, self.forward_speed, self.lateral_speed, self.yaw_speed) - class applyPickPlaceController(ManipulatorRobotAction): def __init__( diff --git a/src/scenic/simulators/isaac/backends/__init__.py b/src/scenic/simulators/isaac/backends/__init__.py index 5d6eb5ca5..675441752 100644 --- a/src/scenic/simulators/isaac/backends/__init__.py +++ b/src/scenic/simulators/isaac/backends/__init__.py @@ -5,9 +5,10 @@ "core_51": "scenic.simulators.isaac.backends.core_51:Core51Backend", "experimental_51": "scenic.simulators.isaac.backends.experimental_51:Experimental51Backend", "experimental_60": "scenic.simulators.isaac.backends.experimental_60:Experimental60Backend", + "lab": "scenic.simulators.isaac.backends.lab:LabBackend", } _INSTANCES = {} -DEFAULT_BACKEND_NAME = "core_51" +DEFAULT_BACKEND_NAME = "experimental_60" _DEFAULT_BACKEND = DEFAULT_BACKEND_NAME diff --git a/src/scenic/simulators/isaac/backends/base.py b/src/scenic/simulators/isaac/backends/base.py index d3ce61aa8..79655e53e 100644 --- a/src/scenic/simulators/isaac/backends/base.py +++ b/src/scenic/simulators/isaac/backends/base.py @@ -1,19 +1,11 @@ import atexit import asyncio -from dataclasses import dataclass import os import numpy as np import scenic.simulators.isaac.utils as scenic_utils -@dataclass(frozen=True) -class IsaacArticulationAction: - """Backend-neutral articulation action payload.""" - - kwargs: dict - - class IsaacBackend: """Interface implemented by Isaac Sim API backends.""" @@ -122,7 +114,7 @@ def release_world(self, world): pass def add_object(self, world, obj): - raise NotImplementedError + pass async def convert(self, in_file, out_file, load_materials=False): import omni.kit.asset_converter @@ -248,35 +240,52 @@ def isaac_quat_to_scenic_euler_angles(self, quat): yaw, pitch, roll = R.from_quat(q_xyzw).as_euler("ZXY", degrees=False) return float(yaw), float(pitch), float(roll) - - def _vec3_to_np(self, vec): - return np.array([float(vec[0]), float(vec[1]), float(vec[2])], dtype=float) - def compute_prim_world_bbox(self, prim_path): """Return world-space bbox min, max, center, and size for a prim.""" - from pxr import Usd, UsdGeom from isaacsim.core.utils import prims prim = prims.get_prim_at_path(prim_path) if prim is None or not prim.IsValid(): raise ValueError(f"invalid prim path: {prim_path}") + return self.compute_prim_bbox(prim) + + def compute_usd_asset_bbox(self, usd_path): + """Return the composed bbox of a USD asset referenced at the origin.""" + from pxr import Usd + + stage = Usd.Stage.CreateInMemory() + prim = stage.DefinePrim("/Asset", "Xform") + prim.GetReferences().AddReference(os.fspath(usd_path)) + return self.compute_prim_bbox(prim) + + def compute_prim_bbox(self, prim): + """Return world-space bbox min, max, center, and size for a USD prim.""" + from pxr import Usd, UsdGeom + cache = UsdGeom.BBoxCache( Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render, UsdGeom.Tokens.proxy], useExtentsHint=True, ) + # bbox = bbox_cache.ComputeLocalBound(prim) + # bbox_range = bbox.ComputeAlignedRange() + + # native_min = np.asarray(bbox_range.GetMin(), dtype=float) + # native_max = np.asarray(bbox_range.GetMax(), dtype=float) + # native_size = native_max - native_min + # native_center = (native_min + native_max) / 2.0 + box = cache.ComputeWorldBound(prim).ComputeAlignedBox() - mn = self._vec3_to_np(box.GetMin()) - mx = self._vec3_to_np(box.GetMax()) + mn = np.asarray(box.GetMin(), dtype=float) + mx = np.asarray(box.GetMax(), dtype=float) center = (mn + mx) * 0.5 size = mx - mn return mn, mx, center, size - def rotate_vector_by_wxyz_quat(self, quat_wxyz, vec): """Rotate a vector by an Isaac/Usd wxyz quaternion.""" from scipy.spatial.transform import Rotation as R @@ -291,7 +300,6 @@ def rotate_vector_by_wxyz_quat(self, quat_wxyz, vec): return R.from_quat(q_xyzw).apply(vec) - def compute_usd_scale_and_root_position(self, obj, prim_path, scenic_position, orientation): """Compute local USD scale so the referenced asset matches Scenic dimensions. @@ -302,12 +310,47 @@ def compute_usd_scale_and_root_position(self, obj, prim_path, scenic_position, o native_center: measured unscaled USD bbox center relative to the root placement """ _, _, native_center, native_size = self.compute_prim_world_bbox(prim_path) + return self.compute_scale_and_root_position( + obj, + native_center, + native_size, + scenic_position, + orientation, + ) + def compute_usd_asset_scale_and_root_position( + self, + obj, + usd_path, + scenic_position, + orientation, + ): + """Compute Scenic scale and root position before a USD asset is spawned.""" + _, _, native_center, native_size = self.compute_usd_asset_bbox(usd_path) + return self.compute_scale_and_root_position( + obj, + native_center, + native_size, + scenic_position, + orientation, + ) + + def compute_scale_and_root_position( + self, + obj, + native_center, + native_size, + scenic_position, + orientation, + ): + """Compute scale and root position from an asset's native bounding box.""" desired_size = np.array( [float(obj.width), float(obj.length), float(obj.height)], dtype=float, ) + native_center = np.asarray(native_center, dtype=float) + native_size = np.asarray(native_size, dtype=float) local_scale = desired_size / native_size # avoid tiny numerical scale changes when dimensions already match. @@ -335,10 +378,7 @@ def create_generic_object(self, obj): def create_robot(self, obj): raise NotImplementedError - def create_create3(self, obj): - raise NotImplementedError - - def create_kaya(self, obj): + def create_wheeled_robot(self, obj): raise NotImplementedError def create_franka_panda(self, obj): @@ -353,6 +393,22 @@ def apply_robot_control(self, sim, obj, command): def apply_wheeled_control(self, sim, obj, command): raise NotImplementedError + def apply_articulation_action(self, sim, obj, action): + raise NotImplementedError + + def articulation_dof_names(self, sim, obj): + raise NotImplementedError + + def articulation_dof_indices(self, sim, obj, names): + dof_names = self.articulation_dof_names(sim, obj) + return [dof_names.index(name) for name in names] + + def get_object_pose(self, sim, obj): + raise NotImplementedError + + def set_object_pose(self, sim, obj, position, orientation=None): + raise NotImplementedError + def move_franka_pick_place( self, sim, @@ -368,4 +424,4 @@ def get_physics_properties(self, world, obj): raise NotImplementedError def articulation_action(self, **kwargs): - return IsaacArticulationAction(dict(kwargs)) + return dict(kwargs) diff --git a/src/scenic/simulators/isaac/backends/core_51.py b/src/scenic/simulators/isaac/backends/core_51.py index 8484c53bc..6809c0007 100644 --- a/src/scenic/simulators/isaac/backends/core_51.py +++ b/src/scenic/simulators/isaac/backends/core_51.py @@ -1,14 +1,22 @@ import math import os - import numpy as np -from scenic.simulators.isaac.backends.base import IsaacArticulationAction, IsaacBackend +from scenic.simulators.isaac.backends.base import IsaacBackend import scenic.simulators.isaac.utils as scenic_utils +class _AckermannControllerAdapter: + def __init__(self, controller): + self.controller = controller + + def forward(self, command): + action = self.controller.forward(command=command) + return action.joint_positions, action.joint_velocities + + class Core51Backend(IsaacBackend): - """Isaac Sim 5.1 backend implemented with the current Core API.""" + """Isaac Sim 5.1.0 backend implemented with the current Core API.""" name = "core_51" @@ -45,6 +53,17 @@ def initialize_physics(self, world, objects): isaac_obj = world.scene.get_object(obj.name) if hasattr(isaac_obj, "initialize"): isaac_obj.initialize() + if getattr(obj, "wheel_controller", None) in { + "differential", + "holonomic", + "ackermann", + }: + obj.wheel_dof_indices = list(isaac_obj.wheel_dof_indices) + if obj.wheel_controller == "ackermann": + obj.steering_dof_indices = [ + isaac_obj.get_dof_index(name) + for name in obj.steering_dof_names + ] def play_world(self, world): world.play() @@ -62,7 +81,7 @@ def release_world(self, world): World.clear_instance() - def add_object(self, world, obj): + def add_object(self, world, obj, *, scenic_obj=None): world.scene.add(obj) def ensure_environment_mesh_paths( @@ -201,6 +220,13 @@ def create_robot(self, obj): from isaacsim.core.api.robots import Robot from isaacsim.core.utils.stage import add_reference_to_stage + if getattr(obj, "wheel_controller", None) in { + "differential", + "holonomic", + "ackermann", + }: + return self.create_wheeled_robot(obj) + if obj.control: obj.controller = self.create_controller( obj.control, f"{obj.name}_controller" @@ -217,78 +243,91 @@ def create_robot(self, obj): prim_path=prim_path, name=obj.name, position=scenic_utils.vectorToArray(obj.position), - orientation=self.scenic_to_isaac_orientation(obj.orientation), - ) - - def create_create3(self, obj): - from isaacsim.robot.wheeled_robots.controllers.differential_controller import ( - DifferentialController, - ) - from isaacsim.robot.wheeled_robots.robots import WheeledRobot - - obj.controller = DifferentialController( - name=f"{obj.name}_controller", - wheel_radius=0.03575, - wheel_base=0.233, - ) - wrapper = WheeledRobot( - prim_path=f"/World/{obj.name}", - name=obj.name, - create_robot=True, orientation=self.scenic_to_isaac_orientation( - obj.orientation, initial_rotation=[0, 0, 0] + obj.orientation, initial_rotation=obj.initial_rotation ), - position=scenic_utils.vectorToArray(obj.position), - usd_path=self.asset_path("Isaac/Robots/iRobot/Create3/create_3.usd"), - wheel_dof_names=["left_wheel_joint", "right_wheel_joint"], ) - if obj.color: - self.apply_visual_material(wrapper, obj) - return wrapper - def create_kaya(self, obj): - from isaacsim.robot.wheeled_robots.controllers.holonomic_controller import ( + def create_wheeled_robot(self, obj): + from isaacsim.robot.wheeled_robots.controllers import ( + AckermannController, + DifferentialController, HolonomicController, ) - from isaacsim.robot.wheeled_robots.robots import WheeledRobot - from isaacsim.robot.wheeled_robots.robots.holonomic_robot_usd_setup import ( + from isaacsim.robot.wheeled_robots.robots import ( HolonomicRobotUsdSetup, + WheeledRobot, ) prim_path = f"/World/{obj.name}" + usd_path = ( + self.asset_path(obj.isaac_asset_path) + if obj.isaac_asset_path + else os.path.abspath(obj.usd_path) + ) wrapper = WheeledRobot( prim_path=prim_path, name=obj.name, - wheel_dof_names=["axle_0_joint", "axle_1_joint", "axle_2_joint"], + wheel_dof_names=obj.wheel_dof_names, create_robot=True, - usd_path=self.asset_path("Isaac/Robots/NVIDIA/Kaya/kaya.usd"), + usd_path=usd_path, position=scenic_utils.vectorToArray(obj.position), orientation=self.scenic_to_isaac_orientation( - obj.orientation, initial_rotation=[np.pi / 2, 0, 0] + obj.orientation, initial_rotation=obj.initial_rotation ), ) - kaya_setup = HolonomicRobotUsdSetup( - robot_prim_path=prim_path, - com_prim_path=f"/World/{obj.name}/base_link/control_offset", - ) - ( - wheel_radius, - wheel_positions, - wheel_orientations, - mecanum_angles, - wheel_axis, - up_axis, - ) = kaya_setup.get_holonomic_controller_params() - obj.controller = HolonomicController( - name="holonomic_controller", - wheel_radius=wheel_radius, - wheel_positions=wheel_positions, - wheel_orientations=wheel_orientations, - mecanum_angles=mecanum_angles, - wheel_axis=wheel_axis, - up_axis=up_axis, - ) + if obj.wheel_controller == "differential": + obj.controller = DifferentialController( + name=f"{obj.name}_controller", + wheel_radius=obj.wheel_radius, + wheel_base=obj.wheel_base, + ) + elif obj.wheel_controller == "holonomic": + holonomic_setup = HolonomicRobotUsdSetup( + robot_prim_path=prim_path, + com_prim_path=f"{prim_path}/base_link/control_offset", + ) + ( + wheel_radius, + wheel_positions, + wheel_orientations, + mecanum_angles, + wheel_axis, + up_axis, + ) = holonomic_setup.get_holonomic_controller_params() + obj.controller = HolonomicController( + name=f"{obj.name}_controller", + wheel_radius=wheel_radius, + wheel_positions=wheel_positions, + wheel_orientations=wheel_orientations, + mecanum_angles=mecanum_angles, + wheel_axis=wheel_axis, + up_axis=up_axis, + max_linear_speed=getattr(obj, "max_linear_speed", 0.5), + max_angular_speed=getattr(obj, "max_angular_speed", 0.8), + max_wheel_speed=getattr(obj, "max_wheel_speed", 10.0), + ) + elif obj.wheel_controller == "ackermann": + steering_dof_names = getattr(obj, "steering_dof_names", None) + if not steering_dof_names: + raise ValueError( + f"Ackermann robot {obj.name} requires steering_dof_names, " + "usually [front_left_steering_joint, front_right_steering_joint]." + ) + + obj.steering_dof_names = steering_dof_names + front_wheel_radius = getattr(obj, "front_wheel_radius", obj.wheel_radius) + back_wheel_radius = getattr(obj, "back_wheel_radius", obj.wheel_radius) + obj.controller = _AckermannControllerAdapter( + AckermannController( + name=f"{obj.name}_controller", + wheel_base=obj.wheel_base, + track_width=obj.track_width, + front_wheel_radius=front_wheel_radius, + back_wheel_radius=back_wheel_radius, + ) + ) if obj.color: self.apply_visual_material(wrapper, obj) @@ -338,11 +377,96 @@ def create_ground_plane(self, obj): def apply_robot_control(self, sim, obj, command): robot = sim.world.scene.get_object(obj.name) - robot.apply_action(self._to_core_articulation_action(obj.controller.forward(command=command))) + if obj.controller is None: + return + if getattr(obj, "wheel_controller", None) in { + "differential", + "holonomic", + "ackermann", + }: + self.apply_wheeled_control(sim, obj, command) + return + + action = obj.controller.forward(command=command) + robot.apply_action(self._to_core_articulation_action(action)) def apply_wheeled_control(self, sim, obj, command): wheeled_robot = sim.world.scene.get_object(obj.name) - wheeled_robot.apply_wheel_actions(obj.controller.forward(command=command)) + if obj.controller is None: + return + + if obj.wheel_controller in {"differential", "holonomic"}: + wheeled_robot.apply_wheel_actions(obj.controller.forward(command=command)) + return + + if obj.wheel_controller == "ackermann": + from isaacsim.core.utils.types import ArticulationAction + + steering_positions, wheel_velocities = obj.controller.forward(command) + wheeled_robot.apply_action( + ArticulationAction( + joint_positions=steering_positions, + joint_indices=obj.steering_dof_indices, + ) + ) + wheeled_robot.apply_wheel_actions( + ArticulationAction(joint_velocities=wheel_velocities) + ) + return + + action = obj.controller.forward(command=command) + wheeled_robot.apply_action(self._to_core_articulation_action(action)) + + def apply_articulation_action(self, sim, obj, action): + robot = sim.world.scene.get_object(obj.name) + for field, index_field in ( + ("joint_positions", "joint_position_indices"), + ("joint_velocities", "joint_velocity_indices"), + ("joint_efforts", "joint_effort_indices"), + ): + split_action = self._core_action_for_field(action, field, index_field) + if split_action is not None: + robot.apply_action(split_action) + + def _core_action_for_field(self, action, field, index_field): + values = action.get(field) + if values is None: + return None + + from isaacsim.core.utils.types import ArticulationAction + + indices = action.get( + index_field, + action.get("joint_indices", action.get("dof_indices")), + ) + return ArticulationAction(**{field: values, "joint_indices": indices}) + + def articulation_dof_names(self, sim, obj): + robot = sim.world.scene.get_object(obj.name) + names = getattr(robot, "dof_names", None) + if names is not None: + return list(names) + names = getattr(robot, "_dof_names", None) + if names is not None: + return list(names) + raise RuntimeError(f"unable to read DOF names for {obj.name}") + + def get_object_pose(self, sim, obj): + wrapper = sim.world.scene.get_object(obj.name) + position, orientation = wrapper.get_world_pose() + return np.array(position, dtype=float), np.array(orientation, dtype=float) + + def set_object_pose(self, sim, obj, position, orientation=None): + wrapper = sim.world.scene.get_object(obj.name) + position = np.array(position, dtype=float) + if orientation is None: + _, orientation = wrapper.get_world_pose() + orientation = np.array(orientation, dtype=float) + wrapper.set_world_pose(position=position, orientation=orientation) + if hasattr(wrapper, "set_linear_velocity"): + wrapper.set_linear_velocity(np.zeros(3, dtype=float)) + if hasattr(wrapper, "set_angular_velocity"): + wrapper.set_angular_velocity(np.zeros(3, dtype=float)) def move_franka_pick_place( self, @@ -405,8 +529,6 @@ def get_physics_properties(self, world, obj): } def _to_core_articulation_action(self, action): - if not isinstance(action, IsaacArticulationAction): - return action from isaacsim.core.utils.types import ArticulationAction - return ArticulationAction(**action.kwargs) + return ArticulationAction(**action) diff --git a/src/scenic/simulators/isaac/backends/core_51_usd_to_mesh.py b/src/scenic/simulators/isaac/backends/core_51_usd_to_mesh.py index 3743a9fe5..417dd80e0 100644 --- a/src/scenic/simulators/isaac/backends/core_51_usd_to_mesh.py +++ b/src/scenic/simulators/isaac/backends/core_51_usd_to_mesh.py @@ -1,11 +1,3 @@ -# Copyright (c) 2020-2024, NVIDIA CORPORATION. All rights reserved. -# -# NVIDIA CORPORATION and its licensors retain all intellectual property -# and proprietary rights in and to this software, related documentation -# and any modifications thereto. Any use, reproduction, disclosure or -# distribution of this software and related documentation without an express -# license agreement from NVIDIA CORPORATION is strictly prohibited. - import json import os from pathlib import Path diff --git a/src/scenic/simulators/isaac/backends/experimental_51.py b/src/scenic/simulators/isaac/backends/experimental_51.py index e7d17f099..8c0b65ebd 100644 --- a/src/scenic/simulators/isaac/backends/experimental_51.py +++ b/src/scenic/simulators/isaac/backends/experimental_51.py @@ -1,10 +1,7 @@ from dataclasses import dataclass, field from scenic.simulators.isaac.backends.base import IsaacBackend -from scenic.simulators.isaac.backends.experimental_60 import ( - Experimental60Backend, - ExperimentalObjectHandle, -) +from scenic.simulators.isaac.backends.experimental_60 import Experimental60Backend @dataclass @@ -83,6 +80,5 @@ def release_world(self, world): World.clear_instance() - def add_object(self, world, obj): - if isinstance(obj, ExperimentalObjectHandle): - world.objects[obj.name] = obj + def add_object(self, world, obj, *, scenic_obj=None): + world.objects[scenic_obj.name] = obj diff --git a/src/scenic/simulators/isaac/backends/experimental_60.py b/src/scenic/simulators/isaac/backends/experimental_60.py index dda64f6dd..241240769 100644 --- a/src/scenic/simulators/isaac/backends/experimental_60.py +++ b/src/scenic/simulators/isaac/backends/experimental_60.py @@ -3,30 +3,21 @@ import os import numpy as np -from scenic.simulators.isaac.backends.base import IsaacArticulationAction, IsaacBackend +from scenic.simulators.isaac.backends.base import IsaacBackend import scenic.simulators.isaac.utils as scenic_utils -@dataclass -class ExperimentalObjectHandle: - name: str - prim_path: str - wrapper: object - kind: str - metadata: dict = field(default_factory=dict) - - @dataclass class ExperimentalWorld: app: object timestep: float + render: bool = False objects: dict = field(default_factory=dict) simulation_time: float = 0.0 def get_object(self, name): return self.objects[name] - @dataclass class FrankaPickPlaceState: stage: int = 0 @@ -83,21 +74,26 @@ def _differential_inverse_kinematics( @ error ).squeeze(-1) - class Experimental60Backend(IsaacBackend): - """Isaac Sim 6.0 backend implemented with Core Experimental APIs.""" + """Isaac Sim 6.0.0 backend implemented with Core Experimental APIs.""" name = "experimental_60" def create_world(self, timestep): - self._ensure_stage() - return ExperimentalWorld(app=self._simulation_app, timestep=timestep) - - def _ensure_stage(self): import isaacsim.core.experimental.utils.stage as stage_utils + from pxr import UsdPhysics if stage_utils.get_current_stage() is None: stage_utils.create_new_stage(template="sunlight") + + stage = stage_utils.get_current_stage() + stage_utils.set_stage_up_axis("Z") + stage_utils.set_stage_units(meters_per_unit=1.0) + + if not stage.GetPrimAtPath("/World/physicsScene").IsValid(): + UsdPhysics.Scene.Define(stage, "/World/physicsScene") + + return ExperimentalWorld(app=self._simulation_app, timestep=timestep) def open_environment_stage(self, usd_path): import isaacsim.core.experimental.utils.stage as stage_utils @@ -122,15 +118,14 @@ def _stage_already_open(self, stage_utils, usd_path): return True def enable_extension(self, name): - import omni.kit.app + import isaacsim.core.experimental.utils.app as app_utils - manager = omni.kit.app.get_app().get_extension_manager() - if hasattr(manager, "set_extension_enabled_immediate"): - manager.set_extension_enabled_immediate(name, True) - else: - manager.set_extension_enabled(name, True) + app_utils.enable_extension(name) def initialize_physics(self, world, objects): + from isaacsim.core.simulation_manager import SimulationManager + + SimulationManager.setup_simulation(dt=world.timestep) if world.app is not None: world.app.update() @@ -142,9 +137,16 @@ def play_world(self, world): world.app.update() def step_world(self, world): + from isaacsim.core.simulation_manager import SimulationManager + from isaacsim.core.rendering_manager import RenderingManager + + SimulationManager.step(steps=1) + RenderingManager.render() + if world.app is not None: world.app.update() - world.simulation_time += world.timestep + + world.simulation_time = SimulationManager.get_simulation_time() def stop_and_clear_world(self, world): import isaacsim.core.experimental.utils.stage as stage_utils @@ -152,22 +154,24 @@ def stop_and_clear_world(self, world): timeline = omni.timeline.get_timeline_interface() timeline.stop() + if world.app is not None: + world.app.update() - for handle in list(world.objects.values()): - stage_utils.delete_prim(handle.prim_path) + for name, wrapper in list(world.objects.items()): + prim_path = getattr(wrapper, "prim_path", f"/World/{name}") + try: + stage_utils.delete_prim(prim_path) + except Exception as exc: + pass world.objects.clear() - - def add_object(self, world, obj): - if isinstance(obj, ExperimentalObjectHandle): - world.objects[obj.name] = obj + if world.app is not None: + world.app.update() def run_coroutine(self, coro): - if self._simulation_app is not None and hasattr( - self._simulation_app, "run_coroutine" - ): - return self._simulation_app.run_coroutine(coro) + return self._simulation_app.run_coroutine(coro) - return super().run_coroutine(coro) + def add_object(self, world, obj, *, scenic_obj=None): + world.objects[scenic_obj.name] = obj def ensure_environment_mesh_paths( self, @@ -229,19 +233,27 @@ def create_generic_object(self, obj): from isaacsim.core.experimental.prims import RigidPrim, XformPrim prim_path = f"/World/{obj.name}" + asset_prim_path = f"{prim_path}/asset" + usd_path = ( self.asset_path(obj.isaac_asset_path) if obj.isaac_asset_path else os.path.abspath(obj.usd_path) ) - stage_utils.add_reference_to_stage(usd_path=usd_path, path=prim_path) + + stage_utils.define_prim(prim_path, "Xform") + + stage_utils.add_reference_to_stage(usd_path=usd_path, path=asset_prim_path) scenic_position = scenic_utils.vectorToArray(obj.position) orientation = self.scenic_to_isaac_orientation(obj.orientation) + + # Geometry is under /World/ObjectName/asset. geometry_paths = self._geometry_paths_under(prim_path) self._apply_collisions_to_geometry(geometry_paths) # Compute scale from Scenic dimensions to native USD dimensions. + # This should compute the bbox of the full parent, including the asset child. root_position, local_scale, native_size, native_center = ( self.compute_usd_scale_and_root_position( obj, @@ -256,29 +268,33 @@ def create_generic_object(self, obj): prim_path, positions=root_position, orientations=orientation, + scales=local_scale, reset_xform_op_properties=True, ) + if obj.mass is not None: - wrapper.set_masses(obj.mass) + wrapper.set_masses(np.asarray([obj.mass], dtype=np.float32)) + if obj.density is not None: - wrapper.set_densities(obj.density) + wrapper.set_densities(np.asarray([obj.density], dtype=np.float32)) + velocity = scenic_utils.vectorToArray(obj.velocity) wrapper.set_velocities(linear_velocities=velocity) + else: wrapper = XformPrim( prim_path, positions=root_position, orientations=orientation, + scales=local_scale, reset_xform_op_properties=True, ) self.disable_rigid_body(prim_path) - - wrapper.set_world_poses(positions=root_position, orientations=orientation) - wrapper.set_local_scales(local_scale) if obj.color: self.apply_visual_material(wrapper, obj, geometry_paths=geometry_paths) - return ExperimentalObjectHandle(obj.name, prim_path, wrapper, "object") + + return wrapper def _geometry_paths_under(self, prim_path): import isaacsim.core.experimental.utils.stage as stage_utils @@ -329,6 +345,9 @@ def create_robot(self, obj): import isaacsim.core.experimental.utils.stage as stage_utils from isaacsim.core.experimental.prims import Articulation + if getattr(obj, "wheel_controller", None) in {"differential", "holonomic", "ackermann"}: + return self.create_wheeled_robot(obj) + prim_path = f"/World/{obj.name}" usd_path = ( self.asset_path(obj.isaac_asset_path) @@ -339,69 +358,145 @@ def create_robot(self, obj): wrapper = Articulation( prim_path, positions=scenic_utils.vectorToArray(obj.position), - orientations=self.scenic_to_isaac_orientation(obj.orientation), + orientations=self.scenic_to_isaac_orientation( + obj.orientation, initial_rotation=obj.initial_rotation + ), reset_xform_op_properties=True, ) if obj.control: obj.controller = obj.control - return ExperimentalObjectHandle(obj.name, prim_path, wrapper, "articulation") - - def create_create3(self, obj): + if obj.color: + self.apply_visual_material(wrapper, obj, geometry_paths=self._geometry_paths_under(prim_path)) + return wrapper + + def create_wheeled_robot(self, obj): import isaacsim.core.experimental.utils.stage as stage_utils - from isaacsim.core.experimental.prims import Articulation + from isaacsim.robot.experimental.wheeled_robots.robots import ( + WheeledRobot, + HolonomicRobotUsdSetup, + ) + from isaacsim.robot.experimental.wheeled_robots.controllers import ( + DifferentialController, + HolonomicController, + AckermannController, + ) prim_path = f"/World/{obj.name}" - stage_utils.add_reference_to_stage( - usd_path=self.asset_path("Isaac/Robots/iRobot/Create3/create_3.usd"), - path=prim_path, + usd_path = ( + self.asset_path(obj.isaac_asset_path) + if obj.isaac_asset_path + else os.path.abspath(obj.usd_path) ) - wrapper = Articulation( - prim_path, + + wrapper = WheeledRobot( + paths=prim_path, + wheel_dof_names=obj.wheel_dof_names, + usd_path=usd_path, positions=scenic_utils.vectorToArray(obj.position), - orientations=self.scenic_to_isaac_orientation(obj.orientation), - reset_xform_op_properties=True, + orientations=self.scenic_to_isaac_orientation(obj.orientation, initial_rotation=obj.initial_rotation) ) - metadata = { - "controller": "differential", - "wheel_radius": 0.03575, - "wheel_base": 0.233, - "wheel_dof_names": ["left_wheel_joint", "right_wheel_joint"], - } - if obj.color: - self.apply_visual_material( - wrapper, obj, geometry_paths=self._geometry_paths_under(prim_path) + + obj.wheel_dof_indices = wrapper.get_dof_indices(obj.wheel_dof_names) + + + if obj.wheel_controller == "differential": + obj.controller = DifferentialController( + wheel_radius=obj.wheel_radius, + wheel_base=obj.wheel_base, + ) + elif obj.wheel_controller == "holonomic": + holonomic_setup = HolonomicRobotUsdSetup( + robot_prim_path=prim_path, + com_prim_path=f"/World/{obj.name}/base_link/control_offset", + ) + ( + wheel_radius, + wheel_positions, + wheel_orientations, + mecanum_angles, + wheel_axis, + up_axis, + ) = holonomic_setup.get_holonomic_controller_params() + + obj.controller = HolonomicController( + wheel_radius=wheel_radius, + wheel_positions=wheel_positions, + wheel_orientations=wheel_orientations, + mecanum_angles=mecanum_angles, + wheel_axis=wheel_axis, + up_axis=up_axis, + max_linear_speed=getattr(obj, "max_linear_speed", 0.5), + max_angular_speed=getattr(obj, "max_angular_speed", 0.8), + max_wheel_speed=getattr(obj, "max_wheel_speed", 10.0), ) - return ExperimentalObjectHandle(obj.name, prim_path, wrapper, "articulation", metadata) + elif obj.wheel_controller == "ackermann": + steering_dof_names = getattr(obj, "steering_dof_names", None) + if not steering_dof_names: + raise ValueError( + f"Ackermann robot {obj.name} requires steering_dof_names, " + "usually [front_left_steering_joint, front_right_steering_joint]." + ) - def create_kaya(self, obj): - import isaacsim.core.experimental.utils.stage as stage_utils - from isaacsim.core.experimental.prims import Articulation + obj.steering_dof_names = steering_dof_names + obj.steering_dof_indices = wrapper.get_dof_indices(steering_dof_names) + + # If the user only exposes obj.wheel_radius in Scenic, use it for both front/back. + front_wheel_radius = getattr(obj, "front_wheel_radius", obj.wheel_radius) + back_wheel_radius = getattr(obj, "back_wheel_radius", obj.wheel_radius) + + obj.controller = AckermannController( + wheel_base=obj.wheel_base, + track_width=obj.track_width, + front_wheel_radius=front_wheel_radius, + back_wheel_radius=back_wheel_radius, + ) + else: + obj.controller = obj.control - prim_path = f"/World/{obj.name}" - stage_utils.add_reference_to_stage( - usd_path=self.asset_path("Isaac/Robots/NVIDIA/Kaya/kaya.usd"), - path=prim_path, - ) - wrapper = Articulation( - prim_path, - positions=scenic_utils.vectorToArray(obj.position), - orientations=self.scenic_to_isaac_orientation( - obj.orientation, initial_rotation=[np.pi / 2, 0, 0] - ), - reset_xform_op_properties=True, - ) - metadata = { - "controller": "holonomic", - "wheel_radius": 0.04, - "base_radius": 0.125, - "wheel_dof_names": ["axle_0_joint", "axle_1_joint", "axle_2_joint"], - "wheel_angles": [0.0, 2.0 * np.pi / 3.0, 4.0 * np.pi / 3.0], - } if obj.color: - self.apply_visual_material( - wrapper, obj, geometry_paths=self._geometry_paths_under(prim_path) + self.apply_visual_material(wrapper, obj) + + return wrapper + + def apply_robot_control(self, sim, obj, command): + wrapper = sim.world.get_object(obj.name) + if obj.controller is None: + return + if getattr(obj, "wheel_controller", None) in {"differential", "holonomic", "ackermann"}: + self.apply_wheeled_control(sim, obj, command) + return + + action = obj.controller(command) + self._apply_articulation_action(wrapper, action) + + def apply_wheeled_control(self, sim, obj, command): + wrapper = sim.world.get_object(obj.name) + + if obj.controller is None: + return + + wheel_controller = obj.wheel_controller + + if wheel_controller in {"differential", "holonomic"}: + # Differential command: [linear_speed, angular_speed] + # Holonomic command: [forward_speed, lateral_speed, yaw_speed] + wrapper.apply_wheel_actions(obj.controller.forward(command)) + return + elif wheel_controller == "ackermann": + # Ackermann command: [steering_angle, steering_angle_velocity, speed, acceleration, dt] + steering_positions, wheel_velocities = obj.controller.forward(command) + + wrapper.set_dof_position_targets( + steering_positions, + dof_indices=obj.steering_dof_indices, ) - return ExperimentalObjectHandle(obj.name, prim_path, wrapper, "articulation", metadata) + # obj.wheel_dof_names should be ordered as: [front_left, front_right, rear_left, rear_right] + wrapper.apply_wheel_actions(wheel_velocities) + return + + # If the user supplied a custom controller that returns an existing ArticulationAction. + action = obj.controller(command) + self._apply_articulation_action(wrapper, action) def create_franka_panda(self, obj): import isaacsim.core.experimental.utils.stage as stage_utils @@ -409,7 +504,10 @@ def create_franka_panda(self, obj): prim_path = f"/World/{obj.name}" root_position = self._franka_root_position(obj) - root_orientation = self.scenic_to_isaac_orientation(obj.orientation) + root_orientation = self.scenic_to_isaac_orientation( + obj.orientation, + initial_rotation=obj.initial_rotation, + ) stage_utils.add_reference_to_stage( usd_path=self.asset_path( @@ -418,96 +516,57 @@ def create_franka_panda(self, obj): path=prim_path, variants=[("Gripper", "AlternateFinger"), ("Mesh", "Performance")], ) + wrapper = Articulation( prim_path, positions=root_position, orientations=root_orientation, reset_xform_op_properties=True, ) + default_dof_positions = np.array( [0.012, -0.568, 0.0, -2.811, 0.0, 3.037, 0.741, 0.05, 0.05], dtype=float, ) + wrapper.set_default_state(dof_positions=default_dof_positions) end_effector = RigidPrim(f"{prim_path}/panda_hand") end_effector_link_index = wrapper.get_link_indices("panda_hand").list()[0] - metadata = { + + obj._franka_metadata = { + "prim_path": prim_path, "end_effector": end_effector, "end_effector_link_index": end_effector_link_index, "arm_dof_indices": list(range(7)), "arm_dof_count": 7, "gripper_dof_indices": self._dof_indices( - wrapper, ["panda_finger_joint1", "panda_finger_joint2"] + wrapper, + ["panda_finger_joint1", "panda_finger_joint2"], ), "default_dof_positions": default_dof_positions, "open_gripper_positions": np.array([0.05, 0.05], dtype=float), "closed_gripper_positions": np.array([0.01, 0.01], dtype=float), + # Isaac quaternion convention: [w, x, y, z]. "downward_orientation": np.array([0.0, 1.0, 0.0, 0.0], dtype=float), } - return ExperimentalObjectHandle( - obj.name, - prim_path, - wrapper, - "franka", - metadata, - ) - - def _franka_root_position(self, obj): - position = scenic_utils.vectorToArray(obj.position) - position[2] -= obj.height / 2 - return position - def create_ground_plane(self, obj): - from isaacsim.core.experimental.objects import GroundPlane + obj._franka_pick_place_state = None - wrapper = GroundPlane( - "/World/GroundPlane", - sizes=max(obj.width, obj.length), - positions=[0, 0, 0], - ) if obj.color: - self.apply_visual_material(wrapper, obj) - return ExperimentalObjectHandle(obj.name, "/World/GroundPlane", wrapper, "ground") - - def apply_robot_control(self, sim, obj, command): - handle = sim.world.get_object(obj.name) - if obj.controller is None: - return - action = obj.controller(command) - self._apply_articulation_action(handle.wrapper, action) - - def apply_wheeled_control(self, sim, obj, command): - handle = sim.world.get_object(obj.name) - metadata = handle.metadata - if metadata.get("controller") == "differential": - throttle, steering = command - radius = metadata["wheel_radius"] - base = metadata["wheel_base"] - velocities = [ - ((2.0 * throttle) - (steering * base)) / (2.0 * radius), - ((2.0 * throttle) + (steering * base)) / (2.0 * radius), - ] - elif metadata.get("controller") == "holonomic": - forward_speed, lateral_speed, yaw_speed = command - radius = metadata["wheel_radius"] - base_radius = metadata["base_radius"] - velocities = [] - for angle in metadata["wheel_angles"]: - velocities.append( - ( - math.sin(angle) * forward_speed - + math.cos(angle) * lateral_speed - + base_radius * yaw_speed - ) - / radius - ) - else: - raise RuntimeError(f"{obj.name} has no wheeled controller metadata") + self.apply_visual_material( + wrapper, + obj, + geometry_paths=self._geometry_paths_under(prim_path), + ) - dof_indices = self._dof_indices(handle.wrapper, metadata["wheel_dof_names"]) - handle.wrapper.set_dof_velocity_targets(velocities, dof_indices=dof_indices) + return wrapper + def _franka_root_position(self, obj): + position = scenic_utils.vectorToArray(obj.position) + position[2] -= obj.height / 2 + return position + def move_franka_pick_place( self, sim, @@ -517,24 +576,29 @@ def move_franka_pick_place( end_effector_offset=None, end_effector_orientation=None, ): - handle = sim.world.get_object(obj.name) + franka = sim.world.get_object(obj.name) - if obj.controller is None: - obj.controller = FrankaPickPlaceState() - self._reset_franka(handle) - if obj.controller.done: + state = getattr(obj, "_franka_pick_place_state", None) + if state is None: + state = FrankaPickPlaceState() + obj._franka_pick_place_state = state + self._reset_franka(obj, franka) + + if state.done: return if end_effector_orientation is None: end_effector_orientation = obj.end_effector_orientation + if end_effector_offset is None: end_effector_offset = obj.end_effector_offset - end_effector_offset = np.array(end_effector_offset, dtype=float) + end_effector_offset = np.asarray(end_effector_offset, dtype=float) self._move_franka_pick_place_helper( - handle, - obj.controller, + franka, + obj, + state, sim, target_object, goal_position, @@ -544,7 +608,8 @@ def move_franka_pick_place( def _move_franka_pick_place_helper( self, - handle, + franka, + obj, state, sim, target_object, @@ -552,121 +617,195 @@ def _move_franka_pick_place_helper( end_effector_offset, end_effector_orientation=None, ): + metadata = obj._franka_metadata + if state.pick_position is None: - target_handle = sim.world.get_object(target_object.name) - state.pick_position = target_handle.wrapper.get_world_poses()[0].numpy()[0].copy() + target_wrapper = sim.world.get_object(target_object.name) + state.pick_position = target_wrapper.get_world_poses()[0].numpy()[0].copy() + if state.place_position is None: state.place_position = scenic_utils.vectorToArray(goal_position).copy() cube_position = state.pick_position - place = state.place_position - current_position, current_orientation = self._franka_end_effector_pose(handle) + place_position = state.place_position + + current_position, current_orientation = self._franka_end_effector_pose(obj) if end_effector_orientation is not None: - state.end_effector_orientation = np.array(end_effector_orientation, dtype=float).copy() + state.end_effector_orientation = np.asarray( + end_effector_orientation, + dtype=float, + ).copy() elif state.end_effector_orientation is None: - state.end_effector_orientation = handle.metadata["downward_orientation"].copy() + state.end_effector_orientation = metadata["downward_orientation"].copy() + orientation = state.end_effector_orientation phases = [ - (cube_position + np.array([0.0, 0.0, 0.2]), "open", 120), - (cube_position + np.array([0.0, 0.0, 0.1]), "open", 80), + (cube_position + np.array([0.0, 0.0, 0.20]), "open", 120), + (cube_position + np.array([0.0, 0.0, 0.10]), "open", 80), (None, "closed", 50), - (cube_position + np.array([0.0, 0.0, 0.5]), "closed", 150), - (place + np.array([0.0, 0.0, 0.5]), "closed", 180), - (place + np.array([0.0, 0.0, 0.2]), "closed", 90), + (cube_position + np.array([0.0, 0.0, 0.50]), "closed", 150), + (place_position + np.array([0.0, 0.0, 0.50]), "closed", 180), + (place_position + np.array([0.0, 0.0, 0.20]), "closed", 90), (None, "open", 20), ] - target, gripper, steps = phases[state.stage] + target, gripper_state, steps = phases[state.stage] + if target is not None: self._move_franka_end_effector( - handle, - current_position, - current_orientation, - np.array([target + end_effector_offset], dtype=float), - np.array([orientation], dtype=float), + franka, + obj, + current_position=current_position, + current_orientation=current_orientation, + goal_position=np.asarray([target + end_effector_offset], dtype=float), + goal_orientation=np.asarray([orientation], dtype=float), ) - if gripper == "open": - self._set_franka_gripper(handle, "open") - else: - self._set_franka_gripper(handle, "closed") + self._set_franka_gripper(franka, obj, gripper_state) state.stage_steps += 1 if state.stage_steps > steps: - target, gripper, steps = phases[state.stage] print( f"Franka stage={state.stage}, steps={state.stage_steps}, " - f"target={target}, gripper={gripper}" + f"target={target}, gripper={gripper_state}", + flush=True, ) + state.stage += 1 state.stage_steps = 0 + if state.stage >= len(phases): state.done = True - def _reset_franka(self, handle): - handle.wrapper.reset_to_default_state() - handle.wrapper.set_dof_position_targets( - handle.metadata["default_dof_positions"] - ) + def _dof_indices(self, articulation, names): + dof_names = list(articulation.dof_names) + return [dof_names.index(name) for name in names] + + def _reset_franka(self, obj, franka): + metadata = obj._franka_metadata - def _franka_end_effector_pose(self, handle): - position, orientation = handle.metadata["end_effector"].get_world_poses() + franka.reset_to_default_state() + franka.set_dof_position_targets(metadata["default_dof_positions"]) + self._set_franka_gripper(franka, obj, "open") + + + def _franka_end_effector_pose(self, obj): + metadata = obj._franka_metadata + position, orientation = metadata["end_effector"].get_world_poses() return position.numpy(), orientation.numpy() + def _move_franka_end_effector( self, - handle, + franka, + obj, current_position, current_orientation, goal_position, goal_orientation=None, ): - franka = handle.wrapper - metadata = handle.metadata + metadata = obj._franka_metadata + + arm_dof_indices = metadata["arm_dof_indices"] + arm_dof_count = metadata["arm_dof_count"] + current_dof_positions = franka.get_dof_positions().numpy() jacobian_matrices = franka.get_jacobian_matrices().numpy() + jacobian_end_effector = jacobian_matrices[ :, metadata["end_effector_link_index"] - 1, :, - : metadata["arm_dof_count"], + :arm_dof_count, ] + delta_dof_positions = _differential_inverse_kinematics( jacobian_end_effector=jacobian_end_effector, - current_position=current_position, - current_orientation=current_orientation, - goal_position=np.asarray(goal_position, dtype=float).reshape(1, -1), + current_position=np.asarray(current_position, dtype=float).reshape(1, 3), + current_orientation=np.asarray(current_orientation, dtype=float).reshape(1, 4), + goal_position=np.asarray(goal_position, dtype=float).reshape(1, 3), goal_orientation=( None if goal_orientation is None - else np.asarray(goal_orientation, dtype=float).reshape(1, -1) + else np.asarray(goal_orientation, dtype=float).reshape(1, 4) ), ) - dof_position_targets = ( - current_dof_positions[:, metadata["arm_dof_indices"]] - + delta_dof_positions - ) + + if current_dof_positions.ndim == 1: + dof_position_targets = ( + current_dof_positions[arm_dof_indices] + delta_dof_positions[0] + ) + else: + dof_position_targets = ( + current_dof_positions[:, arm_dof_indices] + delta_dof_positions + ) + franka.set_dof_position_targets( dof_position_targets, - dof_indices=metadata["arm_dof_indices"], + dof_indices=arm_dof_indices, ) - def _set_franka_gripper(self, handle, state): - positions = handle.metadata[f"{state}_gripper_positions"] - handle.wrapper.set_dof_position_targets( + + def _set_franka_gripper(self, franka, obj, state): + metadata = obj._franka_metadata + + if state == "open": + positions = metadata["open_gripper_positions"] + elif state == "closed": + positions = metadata["closed_gripper_positions"] + + franka.set_dof_position_targets( positions, - dof_indices=handle.metadata["gripper_dof_indices"], + dof_indices=metadata["gripper_dof_indices"], + ) + + def create_ground_plane(self, obj): + from isaacsim.core.experimental.objects import GroundPlane + + wrapper = GroundPlane( + "/World/GroundPlane", + sizes=max(obj.width, obj.length), + positions=[0, 0, 0], ) + if obj.color: + self.apply_visual_material(wrapper, obj) + return wrapper + + def apply_articulation_action(self, sim, obj, action): + wrapper = sim.world.get_object(obj.name) + self._apply_articulation_action(wrapper, action) + + def articulation_dof_names(self, sim, obj): + wrapper = sim.world.get_object(obj.name) + return list(wrapper.dof_names) + + def get_object_pose(self, sim, obj): + wrapper = sim.world.get_object(obj.name) + position, orientation = wrapper.get_world_poses() + return position.numpy()[0], orientation.numpy()[0] + + def set_object_pose(self, sim, obj, position, orientation=None): + wrapper = sim.world.get_object(obj.name) + position = np.array(position, dtype=float) + if orientation is None: + _, orientation = self.get_object_pose(sim, obj) + orientation = np.array(orientation, dtype=float) + wrapper.set_world_poses(positions=position, orientations=orientation) + if hasattr(wrapper, "set_velocities"): + wrapper.set_velocities( + linear_velocities=np.zeros(3, dtype=float), + angular_velocities=np.zeros(3, dtype=float), + ) def get_physics_properties(self, world, obj): - handle = world.get_object(obj.name) - position, orientation = handle.wrapper.get_world_poses() + wrapper = world.get_object(obj.name) + position, orientation = wrapper.get_world_poses() position = position.numpy()[0] orientation = orientation.numpy()[0] yaw, pitch, roll = self.isaac_quat_to_scenic_euler_angles(orientation) - linear_velocity, angular_velocity = handle.wrapper.get_velocities() + linear_velocity, angular_velocity = wrapper.get_velocities() linear_velocity = linear_velocity.numpy()[0] angular_velocity = angular_velocity.numpy()[0] lx, ly, lz = linear_velocity @@ -683,27 +822,19 @@ def get_physics_properties(self, world, obj): } def _apply_articulation_action(self, articulation, action): - if not isinstance(action, IsaacArticulationAction): - if isinstance(action, dict): - action = IsaacArticulationAction(action) - else: - raise TypeError( - f"{self.name} robot controls must return articulation_action(...) " - "or a dict with joint position/velocity/effort fields" - ) - kwargs = action.kwargs - dof_indices = kwargs.get("joint_indices", kwargs.get("dof_indices")) - if "joint_positions" in kwargs: + dof_indices = action.get("joint_indices", action.get("dof_indices")) + if "joint_positions" in action: articulation.set_dof_position_targets( - kwargs["joint_positions"], dof_indices=dof_indices + action["joint_positions"], + dof_indices=action.get("joint_position_indices", dof_indices), ) - if "joint_velocities" in kwargs: + if "joint_velocities" in action: articulation.set_dof_velocity_targets( - kwargs["joint_velocities"], dof_indices=dof_indices + action["joint_velocities"], + dof_indices=action.get("joint_velocity_indices", dof_indices), + ) + if "joint_efforts" in action: + articulation.set_dof_efforts( + action["joint_efforts"], + dof_indices=action.get("joint_effort_indices", dof_indices), ) - if "joint_efforts" in kwargs: - articulation.set_dof_efforts(kwargs["joint_efforts"], dof_indices=dof_indices) - - def _dof_indices(self, articulation, names): - dof_names = list(articulation.dof_names) - return [dof_names.index(name) for name in names] diff --git a/src/scenic/simulators/isaac/backends/lab.py b/src/scenic/simulators/isaac/backends/lab.py new file mode 100644 index 000000000..8a068ab3e --- /dev/null +++ b/src/scenic/simulators/isaac/backends/lab.py @@ -0,0 +1,536 @@ +from __future__ import annotations + +import os +import re +from urllib.parse import urlparse + +import trimesh + +from scenic.core.regions import MeshVolumeRegion +from scenic.core.simulators import SimulationCreationError +from scenic.simulators.isaac.backends.base import IsaacBackend +import scenic.simulators.isaac.utils as scenic_utils + + +class LabBackend(IsaacBackend): + + name = "lab" + + def __init__(self): + super().__init__() + self.app_launcher = None + + def ensure_app( + self, + *, + headless=False, + device=None, + app_launcher_args=None, + ): + """Launch Isaac Sim once through Isaac Lab's AppLauncher.""" + if self._simulation_app is not None: + return self._simulation_app + + from isaaclab.app import AppLauncher + + launcher_args = {"headless": headless} + if device is not None: + launcher_args["device"] = device + launcher_args.update(app_launcher_args or {}) + + self.app_launcher = AppLauncher(launcher_args) + self._simulation_app = self.app_launcher.app + return self._simulation_app + + def close_app(self): + if self._simulation_app is not None: + self._simulation_app.close() + self._simulation_app = None + self.app_launcher = None + + def asset_prim_path(self, asset_name: str, num_envs: int | None) -> str: + if int(num_envs or 1) == 1: + return f"/World/envs/env_0/{asset_name}" + return f"{{ENV_REGEX_NS}}/{asset_name}" + + def resolve_usd_path(self, source) -> str: + source = os.fspath(source) + + if urlparse(source).scheme: + return source + + if source.startswith("Isaac/"): + return self.asset_path(source) + + return os.path.abspath(source) + + def usd_path_for_object(self, obj) -> str | None: + if getattr(obj, "usd_path", None): + return self.resolve_usd_path(obj.usd_path) + + if getattr(obj, "isaac_asset_path", None): + return self.resolve_usd_path(obj.isaac_asset_path) + + return None + + def ensure_environment_mesh_paths( + self, + environment_usd_path, + environment_mesh_path=None, + environment_info_path=None, + *, + headless=True, + overwrite=False, + ): + default_mesh_path, default_info_path = scenic_utils.default_environment_mesh_paths( + environment_usd_path + ) + mesh_path = ( + scenic_utils.resolvedPath(environment_mesh_path) + if environment_mesh_path + else default_mesh_path + ) + info_path = ( + scenic_utils.resolvedPath(environment_info_path) + if environment_info_path + else default_info_path + ) + + if not overwrite and scenic_utils.environment_outputs_current( + environment_usd_path, mesh_path, info_path + ): + return mesh_path, info_path + + mesh_path.parent.mkdir(parents=True, exist_ok=True) + info_path.parent.mkdir(parents=True, exist_ok=True) + + self.ensure_app(headless=headless) + from isaacsim.core.utils.extensions import enable_extension + + enable_extension("omni.kit.asset_converter") + from scenic.simulators.isaac.backends.core_51_usd_to_mesh import ( + convert_environment_usd, + ) + + convert_environment_usd( + self.kit_usd_path(environment_usd_path), + str(mesh_path), + str(info_path), + overwrite=True, + backend_name=self.name, + open_stage_func=self._open_stage_for_conversion, + ) + return mesh_path, info_path + + def _open_stage_for_conversion(self, usd_path): + import isaacsim.core.experimental.utils.stage as stage_utils + + result = stage_utils.open_stage(usd_path) + return result[0] if isinstance(result, tuple) else bool(result) + + def scenic_pose(self, obj): + pos = getattr(obj, "position", None) + if pos is None: + position = (0.0, 0.0, 0.0) + else: + position = ( + float(getattr(pos, "x", 0.0)), + float(getattr(pos, "y", 0.0)), + float(getattr(pos, "z", 0.0)), + ) + + orientation = self.scenic_to_isaac_orientation( + obj.orientation, + initial_rotation=getattr(obj, "initial_rotation", None), + ) + return position, tuple(float(value) for value in orientation) + + def scenic_color(self, obj): + color = getattr(obj, "color", None) + if color is None: + return None + r, g, b = color[:3] + return (float(r), float(g), float(b)) + + def preview_surface_cfg(self, obj): + color = self.scenic_color(obj) + if color is None: + return None + + import isaaclab.sim as sim_utils + + return sim_utils.PreviewSurfaceCfg( + diffuse_color=color, + roughness=0.5, + metallic=0.0, + ) + + # --------------------------------------------------------------------- + # Asset cfg builders + # --------------------------------------------------------------------- + + def make_environment_cfg(self, environment_usd_path): + """Create a static environment asset cloned into every Lab environment.""" + import isaaclab.sim as sim_utils + from isaaclab.assets import AssetBaseCfg + + usd_path = self.resolve_usd_path(environment_usd_path) + if not urlparse(usd_path).scheme and not os.path.isfile(usd_path): + raise SimulationCreationError( + f"Isaac Lab environment USD does not exist or is not a file: {usd_path!r}" + ) + + return AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/ScenicEnvironment", + spawn=sim_utils.UsdFileCfg(usd_path=usd_path), + ) + + def make_asset_cfg(self, obj, asset_name: str, *, num_envs: int | None, tmp_mesh_dir: str): + blueprint = obj.blueprint + + if blueprint == "GroundPlane": + return self.make_ground_plane_cfg(obj, asset_name, num_envs=num_envs) + + if blueprint == "Robot": + return self.make_robot_cfg(obj, asset_name, num_envs=num_envs) + + return self.make_object_cfg( + obj, + asset_name, + num_envs=num_envs, + tmp_mesh_dir=tmp_mesh_dir, + ) + + def make_ground_plane_cfg(self, obj, asset_name: str, *, num_envs: int | None): + import isaaclab.sim as sim_utils + from isaaclab.assets import AssetBaseCfg + + pos, rot = self.scenic_pose(obj) + + return AssetBaseCfg( + prim_path=self.asset_prim_path(asset_name, num_envs), + spawn=sim_utils.CuboidCfg( + size=(obj.width, obj.length, obj.height), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=self.preview_surface_cfg(obj), + ), + init_state=AssetBaseCfg.InitialStateCfg(pos=pos, rot=rot), + ) + + def make_object_cfg(self, obj, asset_name: str, *, num_envs: int | None, tmp_mesh_dir: str): + import isaaclab.sim as sim_utils + from isaaclab.assets import AssetBaseCfg, RigidObjectCfg + + prim_path = self.asset_prim_path(asset_name, num_envs) + has_usd_asset = bool( + getattr(obj, "usd_path", None) or getattr(obj, "isaac_asset_path", None) + ) + + if not has_usd_asset: + self.convert_mesh_object_to_usd(obj, tmp_mesh_dir) + + usd_path = self.usd_path_for_object(obj) + if usd_path is None: + raise SimulationCreationError( + f"Cannot convert Scenic object {getattr(obj, 'name', obj)!r} " + "into an Isaac Lab asset cfg." + ) + + if has_usd_asset and getattr(obj, "physics", False): + usd_path = self.make_rigid_usd_wrapper( + usd_path, + obj, + asset_name, + tmp_mesh_dir, + ) + + scenic_position, rot = self.scenic_pose(obj) + pos, scale, _, _ = self.compute_usd_asset_scale_and_root_position( + obj, + usd_path, + scenic_position, + rot, + ) + pos = tuple(float(value) for value in pos) + scale = tuple(float(value) for value in scale) + spawn = sim_utils.UsdFileCfg( + usd_path=usd_path, + scale=scale, + visual_material=self.preview_surface_cfg(obj), + ) + + if getattr(obj, "physics", False): + return RigidObjectCfg( + prim_path=prim_path, + spawn=spawn, + init_state=RigidObjectCfg.InitialStateCfg(pos=pos, rot=rot), + ) + + return AssetBaseCfg( + prim_path=prim_path, + spawn=spawn, + init_state=AssetBaseCfg.InitialStateCfg(pos=pos, rot=rot), + ) + + def make_robot_cfg(self, obj, asset_name: str, *, num_envs: int | None): + import isaaclab.sim as sim_utils + from isaaclab.assets import ArticulationCfg + from isaaclab.actuators import ImplicitActuatorCfg + + usd_path = self.usd_path_for_object(obj) + if usd_path is None: + raise SimulationCreationError( + f"Robot {getattr(obj, 'name', obj)!r} needs usd_path or isaac_asset_path." + ) + + pos, rot = self.scenic_pose(obj) + + return ArticulationCfg( + prim_path=self.asset_prim_path(asset_name, num_envs), + spawn=sim_utils.UsdFileCfg( + usd_path=usd_path, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + ), + visual_material=self.preview_surface_cfg(obj), + ), + init_state=ArticulationCfg.InitialStateCfg(pos=pos, rot=rot), + actuators={ + "all_joints": ImplicitActuatorCfg( + joint_names_expr=[".*"], + effort_limit_sim=None, + velocity_limit_sim=None, + stiffness=0.0, + damping=100.0, + ) + }, + ) + + def patch_asset_initial_pose(self, lab_asset_cfg, obj): + if lab_asset_cfg.init_state is None: + initial_state_cls = type(lab_asset_cfg).InitialStateCfg + lab_asset_cfg.init_state = initial_state_cls() + + pos, rot = self.scenic_pose(obj) + lab_asset_cfg.init_state.pos = pos + lab_asset_cfg.init_state.rot = rot + + def convert_mesh_object_to_usd(self, obj, tmp_mesh_dir: str): + os.makedirs(tmp_mesh_dir, exist_ok=True) + + mesh = MeshVolumeRegion( + mesh=obj.shape.mesh, + dimensions=(obj.width, obj.length, obj.height), + ).mesh + + obj_path = os.path.join(tmp_mesh_dir, f"{obj.name}.obj") + usd_path = os.path.join(tmp_mesh_dir, f"{obj.name}.usd") + + trimesh.exchange.export.export_mesh(mesh, obj_path) + + if not self.convert_sync(obj_path, usd_path, load_materials=True): + raise SimulationCreationError( + f"Unable to convert mesh for {obj.name} into a USD asset." + ) + + if getattr(obj, "physics", False): + self.apply_rigid_body_api_to_usd(usd_path, obj) + + obj.usd_path = usd_path + + def make_rigid_usd_wrapper(self, usd_path, obj, asset_name, tmp_mesh_dir): + """Create a local rigid-body wrapper without modifying the source USD.""" + from pxr import Usd, UsdGeom + + os.makedirs(tmp_mesh_dir, exist_ok=True) + wrapper_path = os.path.join(tmp_mesh_dir, f"{asset_name}_rigid.usd") + + stage = Usd.Stage.CreateNew(wrapper_path) + root_prim = UsdGeom.Xform.Define(stage, "/Asset").GetPrim() + root_prim.GetReferences().AddReference(usd_path) + stage.SetDefaultPrim(root_prim) + stage.GetRootLayer().Save() + + self.apply_rigid_body_api_to_usd(wrapper_path, obj) + return wrapper_path + + def apply_rigid_body_api_to_usd(self, usd_path, obj): + """Patch a local USD so Isaac Lab can load it as a RigidObjectCfg. + + RigidObjectCfg requires exactly one UsdPhysics.RigidBodyAPI below its + configured prim. CollisionAPI is applied to mesh children and MassAPI to + the rigid body prim. + """ + from pxr import Usd, UsdGeom, UsdPhysics + + stage = Usd.Stage.Open(usd_path) + if stage is None: + raise SimulationCreationError(f"Could not open USD: {usd_path}") + + root_prim = stage.GetDefaultPrim() + + if not root_prim or not root_prim.IsValid(): + children = list(stage.GetPseudoRoot().GetChildren()) + if not children: + raise SimulationCreationError( + f"USD for {getattr(obj, 'name', obj)!r} has no root prim: {usd_path}" + ) + root_prim = children[0] + stage.SetDefaultPrim(root_prim) + + rigid_body_prims = [ + prim + for prim in Usd.PrimRange(root_prim) + if prim.HasAPI(UsdPhysics.RigidBodyAPI) + ] + if len(rigid_body_prims) > 1: + raise SimulationCreationError( + f"USD asset for {getattr(obj, 'name', obj)!r} contains multiple rigid bodies " + "and cannot be loaded as one Isaac Lab RigidObjectCfg." + ) + + if rigid_body_prims: + rigid_body_prim = rigid_body_prims[0] + else: + UsdPhysics.RigidBodyAPI.Apply(root_prim) + rigid_body_prim = root_prim + + mass_api = UsdPhysics.MassAPI(rigid_body_prim) + if not mass_api: + mass_api = UsdPhysics.MassAPI.Apply(rigid_body_prim) + + mass = getattr(obj, "mass", None) + density = getattr(obj, "density", None) + + if mass is None and density is not None: + width = float(getattr(obj, "width", 1.0)) + length = float(getattr(obj, "length", 1.0)) + height = float(getattr(obj, "height", 1.0)) + mass = float(density) * width * length * height + + if mass is not None: + mass_api.CreateMassAttr(float(mass)) + + mesh_prims = [] + for prim in Usd.PrimRange(root_prim): + if prim.IsA(UsdGeom.Mesh): + mesh_prims.append(prim) + + if not mesh_prims: + # Fallback: try collision on root, although this only works if root has geometry. + if not root_prim.HasAPI(UsdPhysics.CollisionAPI): + UsdPhysics.CollisionAPI.Apply(root_prim) + else: + for mesh_prim in mesh_prims: + if not mesh_prim.HasAPI(UsdPhysics.CollisionAPI): + UsdPhysics.CollisionAPI.Apply(mesh_prim) + + try: + mesh_collision_api = UsdPhysics.MeshCollisionAPI(mesh_prim) + if not mesh_collision_api: + mesh_collision_api = UsdPhysics.MeshCollisionAPI.Apply(mesh_prim) + mesh_collision_api.CreateApproximationAttr("convexHull") + except Exception: + pass + + stage.GetRootLayer().Save() + + def apply_robot_control(self, sim, obj, command): + """Buffer a robot command for application immediately before the Lab step.""" + if getattr(obj, "wheel_controller", None) or callable( + getattr(obj, "control", None) + ): + self.apply_wheeled_control(sim, obj, command) + + def apply_wheeled_control(self, sim, obj, command): + sim._pending_robot_commands[getattr(obj, "name", id(obj))] = (obj, command) + + def apply_differential_drive_command(self, asset, obj, command, *, debug=False): + import torch + + throttle, steering = command + + radius = float(getattr(obj, "wheel_radius", 0.03575)) + base = float(getattr(obj, "wheel_base", 0.233)) + + left_vel = ((2.0 * float(throttle)) - (float(steering) * base)) / (2.0 * radius) + right_vel = ((2.0 * float(throttle)) + (float(steering) * base)) / (2.0 * radius) + + wheel_names = ( + list(getattr(obj, "wheel_dof_names", [])) + or list(getattr(obj, "wheel_joint_names", [])) + or ["left_wheel_joint", "right_wheel_joint"] + ) + + joint_ids = self.find_joint_ids(asset, wheel_names) + + if len(joint_ids) < 2: + if debug: + print("[SCENIC ISAAC LAB DEBUG] Could not resolve wheel joints for", getattr(obj, "name", obj)) + print("[SCENIC ISAAC LAB DEBUG] requested wheel names:", wheel_names) + print("[SCENIC ISAAC LAB DEBUG] available joints:", getattr(asset, "joint_names", None)) + return + + target = torch.zeros((int(asset.num_instances), 2), device=asset.device) + target[:, 0] = left_vel + target[:, 1] = right_vel + + asset.set_joint_velocity_target(target, joint_ids=joint_ids[:2]) + asset.write_data_to_sim() + + def apply_articulation_action(self, sim, obj, action): + asset = sim._asset_for_scenic_object(obj) + self._apply_articulation_action(asset, action) + + def _apply_articulation_action(self, asset, action): + """Apply a generic articulation action to every environment.""" + import torch + + joint_ids = action.get("joint_indices", action.get("dof_indices")) + if hasattr(joint_ids, "tolist"): + joint_ids = joint_ids.tolist() + + applied = False + for field, index_field, setter_name in ( + ("joint_positions", "joint_position_indices", "set_joint_position_target"), + ("joint_velocities", "joint_velocity_indices", "set_joint_velocity_target"), + ("joint_efforts", "joint_effort_indices", "set_joint_effort_target"), + ): + values = action.get(field) + if values is None: + continue + + field_joint_ids = action.get(index_field, joint_ids) + if hasattr(field_joint_ids, "tolist"): + field_joint_ids = field_joint_ids.tolist() + + target = torch.as_tensor(values, dtype=torch.float32, device=asset.device) + if target.ndim == 1: + target = target.unsqueeze(0).repeat(int(asset.num_instances), 1) + + getattr(asset, setter_name)(target, joint_ids=field_joint_ids) + applied = True + + if applied: + asset.write_data_to_sim() + + def find_joint_ids(self, asset, joint_names): + joint_ids, _ = asset.find_joints(joint_names, preserve_order=True) + return joint_ids.tolist() if hasattr(joint_ids, "tolist") else list(joint_ids) + + def _safe_asset_name(self, name: str): + name = str(name) + name = re.sub(r"\W+", "_", name) + if not name: + name = "scenic_object" + if name[0].isdigit(): + name = f"obj_{name}" + return name + + def _tensor_row(self, tensor, env_id: int, default): + if tensor is None: + return tuple(default) + row = tensor[env_id] + if hasattr(row, "detach"): + row = row.detach().cpu().tolist() + return tuple(float(v) for v in row) diff --git a/src/scenic/simulators/isaac/behaviors.scenic b/src/scenic/simulators/isaac/behaviors.scenic index 90e709824..c35526a17 100644 --- a/src/scenic/simulators/isaac/behaviors.scenic +++ b/src/scenic/simulators/isaac/behaviors.scenic @@ -8,11 +8,11 @@ behavior KeepMoving(): while True: if np.linalg.norm(self.speed) < threshold: for i in range(100): - take applyWheeledController(-.2, 0) + take applyController([-.2, 0]) for i in range(50): - take applyWheeledController(0, np.pi) + take applyController([0, np.pi]) else: - take applyWheeledController(.2, 0) + take applyController([.2, 0]) # for the jetbot robot behavior JetbotDrive(): @@ -22,19 +22,25 @@ behavior JetbotDrive(): # for the kaya robot behavior DriveForward(): while True: - take applyHolonomicController(-0.7, 0.0, 0.0) + take applyController([-0.7, 0.0, 0.0]) behavior RandomMovement(): i = 0 while True: - if i >= 0 and i < 500: - take applyHolonomicController(-0.7, 0.0, 0.0) - elif i >= 500 and i < 1000: - take applyHolonomicController(0.0, 0.4, 0.0) - elif i >= 1000 and i < 1100: - take applyHolonomicController(0.0, 0.0, 0.05) - elif i == 1200: + if i < 300: + take applyController([0.4, 0.0, 0.0]) # forward + elif i < 600: + take applyController([-0.4, 0.0, 0.0]) # backward + elif i < 900: + take applyController([0.0, 0.2, 0.0]) # strafe one way + elif i < 1200: + take applyController([0.0, -0.2, 0.0]) # strafe the other way + elif i < 1500: + take applyController([0.0, 0.0, 0.2]) # rotate + else: i = 0 + take applyController([0.0, 0.0, 0.0]) + i += 1 # for the Franka Panda manipulator diff --git a/src/scenic/simulators/isaac/empty_env_cfg.py b/src/scenic/simulators/isaac/empty_env_cfg.py new file mode 100644 index 000000000..b64f725ba --- /dev/null +++ b/src/scenic/simulators/isaac/empty_env_cfg.py @@ -0,0 +1,63 @@ +from __future__ import annotations + +import torch + +from isaaclab.envs import ManagerBasedEnvCfg +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.utils import configclass + + +def zero_obs(env): + """Dummy observation so ManagerBasedEnv has a valid policy observation.""" + return torch.zeros((env.num_envs, 1), device=env.device) + + +@configclass +class ScenicSceneCfg(InteractiveSceneCfg): + """Initially empty scene. + + The Scenic Isaac Lab backend will dynamically add scene fields here before + constructing the ManagerBasedEnv. + """ + + pass + + +@configclass +class ActionsCfg: + pass + + +@configclass +class ObservationsCfg: + @configclass + class PolicyCfg(ObsGroup): + zero = ObsTerm(func=zero_obs) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = True + + policy: PolicyCfg = PolicyCfg() + + +@configclass +class EventsCfg: + pass + + +@configclass +class ScenicEmptyEnvCfg(ManagerBasedEnvCfg): + scene: ScenicSceneCfg = ScenicSceneCfg(num_envs=1, env_spacing=10.0) + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + events: EventsCfg = EventsCfg() + + def __post_init__(self): + self.decimation = 1 + self.sim.dt = 0.01 + + self.viewer.eye = [7.0, 7.0, 6.0] + self.viewer.lookat = [0.0, 0.0, 0.0] diff --git a/src/scenic/simulators/isaac/lab.py b/src/scenic/simulators/isaac/lab.py new file mode 100644 index 000000000..bead1fb4b --- /dev/null +++ b/src/scenic/simulators/isaac/lab.py @@ -0,0 +1,689 @@ +from __future__ import annotations + +import copy +import importlib +import math +import os +import tempfile +from typing import Any + +from scenic.core.simulators import Simulation, SimulationCreationError, Simulator +from scenic.core.vectors import Vector + +from scenic.simulators.isaac.terrain_utils import build_scenic_terrain_data + +from scenic.simulators.isaac.backends import get_backend + +DEFAULT_EMPTY_ENV_CFG = "scenic.simulators.isaac.empty_env_cfg:ScenicEmptyEnvCfg" + + +class IsaacLabSimulator(Simulator): + """Scenic simulator backend for Isaac Lab manager-based environments. + + Supported construction modes: + 1. env= + 2. task="Isaac-Cartpole-v0" or another registered Isaac Lab task + 3. env_cfg= + 4. env_cfg_entry_point="package.module:CfgClass" + + Use cases: + IsaacLabSimulator(task="Isaac-...", num_envs=..., device=...) + or: + IsaacLabSimulator(env_cfg=MyScenicIsaacLabEnvCfg, num_envs=...) + """ + + def __init__( + self, + *, + env: Any | None = None, + task: str | None = None, + env_cfg: Any | None = None, + env_cfg_entry_point: str | None = None, + env_cls: Any | None = None, + timestep: float | None = 0.01, + decimation: int | None = None, + num_envs: int | None = None, + env_spacing: float | None = None, + terrainBorderWidth: float = 20.0, + environmentUSDPath: str | os.PathLike | None = None, + headless: bool = False, + device: str | None = None, + use_fabric: bool = True, + render_mode: str | None = None, + app_launcher_args: dict[str, Any] | None = None, + debug_lifecycle: bool = True, + **kwargs, + ): + super().__init__() + + self.env = env + self.task = task + self.env_cfg = env_cfg + self.env_cfg_entry_point = env_cfg_entry_point + self.env_cls = env_cls + + self.timestep = timestep + self.decimation = decimation + self.num_envs = num_envs + self.env_spacing = env_spacing + self.terrainBorderWidth = terrainBorderWidth + self.environmentUSDPath = environmentUSDPath + + self.headless = headless + self.device = device + self.use_fabric = use_fabric + self.render_mode = render_mode + + self.app_launcher_args = dict(app_launcher_args or {}) + self.app_launcher = None + self.client = None + self.backend = get_backend("lab") + + self.terrain_data = None + + self.debug_lifecycle = debug_lifecycle + + # If an env is already provided, assume the caller owns the app/env. + if self.env is None: + self._ensure_app() + + if ( + self.env is None + and self.task is None + and self.env_cfg is None + and self.env_cfg_entry_point is None + ): + self.env_cfg_entry_point = DEFAULT_EMPTY_ENV_CFG + + def _ensure_app(self): + """Launch Isaac Sim through Isaac Lab's AppLauncher.""" + if self.client is not None: + return self.client + + self.client = self.backend.ensure_app( + headless=self.headless, + device=self.device, + app_launcher_args=self.app_launcher_args, + ) + self.app_launcher = self.backend.app_launcher + return self.client + + def createSimulation(self, scene, **kwargs): + timestep = self.timestep if kwargs.get("timestep") is None else kwargs["timestep"] + kwargs.pop("timestep", None) + + return IsaacLabSimulation( + scene, + self, + env=self.env, + task=self.task, + env_cfg=self.env_cfg, + env_cfg_entry_point=self.env_cfg_entry_point, + env_cls=self.env_cls, + environmentUSDPath=self.environmentUSDPath, + headless=self.headless, + device=self.device, + use_fabric=self.use_fabric, + render_mode=self.render_mode, + terrainBorderWidth=self.terrainBorderWidth, + timestep=timestep, + decimation=self.decimation, + num_envs=self.num_envs, + env_spacing=self.env_spacing, + debug_lifecycle=self.debug_lifecycle, + **kwargs, + ) + + def destroy(self): + super().destroy() + + # If the user passed an existing env, do not close their app here. + if self.env is not None: + return + + if self.client is not None: + self.backend.close_app() + self.client = None + self.app_launcher = None + + +class IsaacLabSimulation(Simulation): + """A Scenic Simulation backed by an Isaac Lab manager-based environment.""" + + def __init__( + self, + scene, + simulator: IsaacLabSimulator, + *, + env: Any | None = None, + task: str | None = None, + env_cfg: Any | None = None, + env_cfg_entry_point: str | None = None, + env_cls: Any | None = None, + environmentUSDPath: str | os.PathLike | None = None, + headless: bool = False, + device: str | None = None, + use_fabric: bool = True, + render_mode: str | None = None, + terrainBorderWidth: float = 20.0, + timestep: float | None, + decimation: int | None = None, + num_envs: int | None = None, + env_spacing: float | None = None, + debug_lifecycle: bool = True, + **kwargs, + ): + kwargs.setdefault("maxSteps", None) + kwargs.setdefault("name", "IsaacLabSimulation") + + self.simulator = simulator + + self.env = env + self._owns_env = env is None + + self.task = task + self.raw_env_cfg = env_cfg + self.env_cfg_entry_point = env_cfg_entry_point + self.env_cls = env_cls + + self.environmentUSDPath = environmentUSDPath + self.headless = headless + self.device = device + self.use_fabric = use_fabric + self.render_mode = render_mode + + self.terrainBorderWidth = terrainBorderWidth + self.timestep = timestep + self.decimation = decimation + self.num_envs = num_envs + self.env_spacing = env_spacing + + self.tmpMeshDir = tempfile.mkdtemp() + + # Scenic objects are collected during Scenic's setup/create-object phase. + # They are not spawned immediately. + self.scenic_objects = [] + self.scenic_existing_objects = [] + self.terrains = [] + + self.terrain_data = None + self.env_cfg = None + + # Maps Scenic object names to Isaac Lab scene entity names. + self._object_name_to_asset_name: dict[str, str] = {} + + # Most recent action/output data. + self._pending_lab_action = None + self._pending_robot_commands = {} + self._last_step_output = None + self._has_reset = False + + # Scenic robot bookkeeping. + self._scenic_robot_asset_names = {} + + if self._owns_env: + self.simulator._ensure_app() + + self.debug_lifecycle = debug_lifecycle + self._step_count = 0 + + self.backend = get_backend("lab") + + super().__init__(scene, timestep=timestep, **kwargs) + + def setup(self): + """Build the Isaac Lab env after Scenic has collected all objects.""" + super().setup() + + if self.env is None: + try: + self.env_cfg = self._build_env_cfg() + self.env = self._make_env(self.env_cfg) + except Exception as exc: + import traceback + + print( + "[SCENIC ISAAC LAB ERROR] Failed while creating Isaac Lab simulation:", + type(exc).__name__, + exc, + flush=True, + ) + traceback.print_exc() + raise + + self._reset_env_once() + + def _build_env_cfg(self): + """Create, validate, and patch an Isaac Lab manager-based env cfg.""" + cfg = self._materialize_env_cfg() + self._validate_manager_based_cfg(cfg) + self._apply_standard_overrides(cfg) + self._apply_scenic_to_env_cfg(cfg) + return cfg + + def _materialize_env_cfg(self): + """Resolve env cfg from task, cfg object/class, or entry point.""" + + if self.raw_env_cfg is not None: + return self._instantiate_cfg(self.raw_env_cfg) + + if self.env_cfg_entry_point is not None: + return self._instantiate_cfg(self.env_cfg_entry_point) + + if self.task is not None: + import isaaclab_tasks + from isaaclab_tasks.utils import parse_env_cfg + + return parse_env_cfg( + self.task, + device=self.device, + num_envs=self.num_envs, + use_fabric=self.use_fabric, + ) + + raise SimulationCreationError( + "IsaacLabSimulation requires one of: env, task, env_cfg, or env_cfg_entry_point." + ) + + def _instantiate_cfg(self, cfg_like): + if isinstance(cfg_like, str): + return self._instantiate_cfg(self._load_entry_point(cfg_like)) + + if isinstance(cfg_like, type): + return cfg_like() + + if callable(cfg_like) and not hasattr(cfg_like, "scene"): + return cfg_like() + + return copy.deepcopy(cfg_like) + + def _load_entry_point(self, entry_point: str): + """Load 'package.module:ClassName'.""" + if ":" not in entry_point: + raise SimulationCreationError( + f"Invalid env_cfg_entry_point {entry_point!r}; expected 'module.submodule:ClassName'." + ) + + module_name, attr_name = entry_point.split(":", 1) + try: + module = importlib.import_module(module_name) + return getattr(module, attr_name) + except Exception as exc: + raise SimulationCreationError( + f"Could not load Isaac Lab env cfg entry point {entry_point!r}." + ) from exc + + def _validate_manager_based_cfg(self, cfg): + from isaaclab.envs import ManagerBasedEnvCfg, ManagerBasedRLEnvCfg + + if not isinstance(cfg, (ManagerBasedEnvCfg, ManagerBasedRLEnvCfg)): + raise SimulationCreationError( + "The Isaac Lab Scenic backend currently expects a manager-based config: " + "ManagerBasedEnvCfg or ManagerBasedRLEnvCfg. " + f"Got {type(cfg).__name__}. If you selected an Isaac Lab task, make sure it is " + "not one of the '-Direct-' environments." + ) + + def _apply_standard_overrides(self, cfg): + """Apply common simulator-level overrides to the env cfg.""" + if self.device is not None and hasattr(cfg, "sim"): + cfg.sim.device = self.device + + if self.timestep is not None and hasattr(cfg, "sim"): + cfg.sim.dt = self.timestep + + if self.decimation is not None and hasattr(cfg, "decimation"): + cfg.decimation = self.decimation + + if self.use_fabric is not None and hasattr(cfg, "sim") and hasattr(cfg.sim, "use_fabric"): + cfg.sim.use_fabric = self.use_fabric + + if self.num_envs is not None and hasattr(cfg, "scene"): + cfg.scene.num_envs = self.num_envs + + if self.env_spacing is not None and hasattr(cfg, "scene"): + cfg.scene.env_spacing = self.env_spacing + + def _apply_scenic_to_env_cfg(self, cfg): + """Patch Isaac Lab cfg using objects sampled by Scenic. + + - If a Scenic object name matches an existing cfg.scene field, patch + that existing Lab asset's initial pose. + - If it does not match, create a new AssetBaseCfg or RigidObjectCfg + from the Scenic object. + - If a Scenic object provides obj.lab_asset_cfg, use that directly. + - Terrain is collected and stored, but the actual TerrainImporterCfg + integration is handled by a custom hook/config. + """ + if not hasattr(cfg, "scene"): + raise SimulationCreationError("Isaac Lab env cfg has no .scene field.") + + if self.terrains: + self.terrain_data = build_scenic_terrain_data( + self.terrains, + border_width=self.terrainBorderWidth, + ) + self.simulator.terrain_data = self.terrain_data + self._install_scenic_terrain_into_cfg(cfg, self.terrain_data) + + if self.environmentUSDPath is not None: + self._add_environment_usd_to_cfg(cfg) + else: + for obj in self.scenic_existing_objects: + self._patch_or_register_scenic_object(cfg, obj, must_exist=True) + + for obj in self.scenic_objects: + self._patch_or_register_scenic_object(cfg, obj, must_exist=False) + + def _install_scenic_terrain_into_cfg(self, cfg, terrain_data): + """Install Scenic terrain into an Isaac Lab env config.""" + if not hasattr(cfg, "scene") or not hasattr(cfg.scene, "terrain"): + raise SimulationCreationError( + "This Isaac Lab env cfg has no cfg.scene.terrain. " + "Scenic Terrain objects require a task/config with a TerrainImporterCfg, " + "or you need to fall back to spawning the terrain mesh as a static collider." + ) + + from scenic.simulators.isaac.lab_env import configure_env_cfg_for_scenic_terrain + configure_env_cfg_for_scenic_terrain(cfg, terrain_data) + + def _add_environment_usd_to_cfg(self, cfg): + """Add the environment USD under each Isaac Lab environment namespace.""" + setattr( + cfg.scene, + "scenic_environment", + self.backend.make_environment_cfg(self.environmentUSDPath), + ) + + def _patch_or_register_scenic_object(self, cfg, obj, *, must_exist: bool): + scenic_name = getattr(obj, "name", obj.__class__.__name__) + asset_name = self.backend._safe_asset_name(scenic_name) + + if hasattr(cfg.scene, asset_name): + lab_asset_cfg = getattr(cfg.scene, asset_name) + self.backend.patch_asset_initial_pose(lab_asset_cfg, obj) + elif must_exist: + raise SimulationCreationError( + f"Existing object {scenic_name!r} has no matching cfg.scene field {asset_name!r}." + ) + else: + lab_asset_cfg = self.backend.make_asset_cfg( + obj, + asset_name, + num_envs=self.num_envs, + tmp_mesh_dir=self.tmpMeshDir, + ) + setattr(cfg.scene, asset_name, lab_asset_cfg) + + self._object_name_to_asset_name[scenic_name] = asset_name + + if getattr(obj, "blueprint", None) == "Robot": + self._scenic_robot_asset_names[scenic_name] = asset_name + + def _make_env(self, cfg): + """Construct the actual Isaac Lab environment.""" + if self.task is not None: + import gymnasium as gym + import isaaclab_tasks # noqa: F401 + + # gym.make uses the task's registered entry_point, usually: + # "isaaclab.envs:ManagerBasedRLEnv" + return gym.make(self.task, cfg=cfg, render_mode=self.render_mode) + + env_cls = self._resolve_env_cls(cfg) + + from isaaclab.envs import ManagerBasedRLEnv + + if issubclass(env_cls, ManagerBasedRLEnv): + return env_cls(cfg=cfg, render_mode=self.render_mode) + + return env_cls(cfg=cfg) + + def _resolve_env_cls(self, cfg): + """Pick ManagerBasedEnv or ManagerBasedRLEnv from the cfg type.""" + if self.env_cls is not None: + if isinstance(self.env_cls, str): + return self._load_entry_point(self.env_cls) + return self.env_cls + + from isaaclab.envs import ( + ManagerBasedEnv, + ManagerBasedEnvCfg, + ManagerBasedRLEnv, + ManagerBasedRLEnvCfg, + ) + + if isinstance(cfg, ManagerBasedRLEnvCfg): + return ManagerBasedRLEnv + if isinstance(cfg, ManagerBasedEnvCfg): + return ManagerBasedEnv + + raise SimulationCreationError(f"Unsupported Isaac Lab cfg type: {type(cfg).__name__}") + + def createObjectInSimulator(self, obj): + """Collect Scenic objects instead of spawning them immediately.""" + blueprint = getattr(obj, "blueprint", None) + + if blueprint == "Terrain": + self.terrains.append(obj) + return None + + if blueprint == "ExistingIsaacSimObject": + self.scenic_existing_objects.append(obj) + return None + + # All objects are translated into env_cfg.scene. + self.scenic_objects.append(obj) + return None + + def executeActions(self, allActions): + """Execute Scenic actions and prepare the Isaac Lab action tensor. + + There are two separate action paths: + + 1. Isaac Lab task action tensor: + - Used by built-in tasks like Cartpole, Ant, etc. + - For now we send zeros unless a policy is connected. + + 2. Scenic-controlled robot commands: + - Used by Scenic behaviors like KeepMoving on Create3. + - Buffered by LabBackend.apply_robot_control and applied directly to the + corresponding Isaac Lab Articulation before env.step(...). + """ + import traceback + + self._pending_robot_commands = {} + + try: + # This calls action.applyTo(obj, self), which calls obj.move(self, ...). + super().executeActions(allActions) + self._pending_lab_action = self.scenic_actions_to_lab_action(allActions) + except Exception as exc: + print( + f"[SCENIC ISAAC LAB ERROR] Exception while applying Scenic actions at step " + f"{self._step_count}: {type(exc).__name__}: {exc}", + flush=True, + ) + traceback.print_exc() + raise + + def scenic_actions_to_lab_action(self, allActions): + """Map Scenic actions into an Isaac Lab action tensor. + + This returns a zero action tensor with the exact shape Isaac Lab expects, + including the num_envs dimension. + """ + if self.env is None: + return None + + import torch + + env = self.env.unwrapped if hasattr(self.env, "unwrapped") else self.env + + if hasattr(env, "action_manager") and hasattr(env.action_manager, "action"): + return torch.zeros_like(env.action_manager.action) + + action_space = getattr(self.env, "action_space", None) + if action_space is None: + return None + + num_envs = int(getattr(env, "num_envs", 1)) + shape = tuple(action_space.shape) + + if len(shape) == 1: + shape = (num_envs, *shape) + + return torch.zeros(shape, device=env.device) + + def step(self): + """Step the Isaac Lab environment once.""" + if self.env is None: + return + + import traceback + import torch + + try: + self._reset_env_once() + + # Apply Scenic-controlled robot commands before stepping the Lab env. + self._apply_pending_robot_commands() + + action = self._pending_lab_action + if action is None: + action = self.scenic_actions_to_lab_action([]) + + with torch.inference_mode(): + self._last_step_output = self.env.step(action) + + self._pending_lab_action = None + self._pending_robot_commands = {} + self._step_count += 1 + + if self.debug_lifecycle and self._step_count % 100 == 0: + print( + f"[SCENIC LAB DEBUG] step={self._step_count}, " + f"sim_time={self._step_count * float(self.timestep):.3f}s" + ) + + except Exception as exc: + print( + f"[SCENIC LAB ERROR] Exception during Isaac Lab step " + f"{self._step_count}: {type(exc).__name__}: {exc}", + flush=True, + ) + traceback.print_exc() + raise + + def _apply_pending_robot_commands(self): + """Apply buffered Scenic robot commands to Isaac Lab articulations.""" + if not self._pending_robot_commands: + return + + for _, (obj, command) in self._pending_robot_commands.items(): + controller = getattr(obj, "wheel_controller", None) + + if controller == "differential": + asset = self._asset_for_scenic_object(obj) + self.backend.apply_differential_drive_command(asset, obj, command) + elif callable(getattr(obj, "control", None)): + self.backend.apply_articulation_action( + self, obj, obj.control(command) + ) + else: + if self.debug_lifecycle: + print( + "[SCENIC ISAAC LAB DEBUG] Unsupported wheeled controller:", + controller, + "for", + getattr(obj, "name", obj), + ) + + def _reset_env_once(self): + if self.env is None or self._has_reset: + return + self.env.reset() + self._has_reset = True + + def getProperties(self, obj, properties): + """Read Scenic-requested properties from Isaac Lab asset buffers.""" + if not getattr(obj, "physics", False): + return {prop: getattr(obj, prop) for prop in properties} + + if self.env is None: + defaults = self._default_physics_values() + return {prop: defaults[prop] for prop in properties} + + asset = self._asset_for_scenic_object(obj) + if asset is None: + defaults = self._default_physics_values() + return {prop: defaults[prop] for prop in properties} + + values = self._physics_values_from_asset(asset, env_id=0) + return {prop: values[prop] for prop in properties} + + def _asset_for_scenic_object(self, obj): + scenic_name = getattr(obj, "name", None) + if scenic_name is None: + return None + + asset_name = self._object_name_to_asset_name.get(scenic_name) + if asset_name is None: + asset_name = self.backend._safe_asset_name(scenic_name) + + env = self.env.unwrapped if hasattr(self.env, "unwrapped") else self.env + return env.scene[asset_name] + + def _physics_values_from_asset(self, asset, env_id: int = 0): + data = getattr(asset, "data", None) + if data is None: + return self._default_physics_values() + + pos = self.backend._tensor_row(getattr(data, "root_pos_w", None), env_id, default=(0.0, 0.0, 0.0)) + quat = self.backend._tensor_row(getattr(data, "root_quat_w", None), env_id, default=(1.0, 0.0, 0.0, 0.0)) + lin_vel = self.backend._tensor_row(getattr(data, "root_lin_vel_w", None), env_id, default=(0.0, 0.0, 0.0)) + ang_vel = self.backend._tensor_row(getattr(data, "root_ang_vel_w", None), env_id, default=(0.0, 0.0, 0.0)) + + yaw, pitch, roll = self.backend.isaac_quat_to_scenic_euler_angles(quat) + + speed = math.sqrt(sum(v * v for v in lin_vel)) + angular_speed = math.sqrt(sum(v * v for v in ang_vel)) + + return dict( + position=Vector(*pos), + velocity=Vector(*lin_vel), + speed=speed, + angularSpeed=angular_speed, + angularVelocity=Vector(*ang_vel), + yaw=yaw, + pitch=pitch, + roll=roll, + ) + + def _default_physics_values(self): + return dict( + position=Vector(0, 0, 0), + velocity=Vector(0, 0, 0), + speed=0.0, + angularSpeed=0.0, + angularVelocity=Vector(0, 0, 0), + yaw=0.0, + pitch=0.0, + roll=0.0, + ) + + def destroy(self): + if self.debug_lifecycle: + result = getattr(self, "result", None) + if result is None: + print("[SCENIC ISAAC LAB DEBUG] destroy called before Simulation.result was set.") + else: + print( + "[SCENIC ISAAC LAB DEBUG] simulation ended:", + "terminationType=", getattr(result, "terminationType", None), + "terminationReason=", getattr(result, "terminationReason", None), + ) + + if self.env is not None and self._owns_env: + self.env.close() + self.env = None diff --git a/src/scenic/simulators/isaac/lab_env.py b/src/scenic/simulators/isaac/lab_env.py new file mode 100644 index 000000000..8fb74a425 --- /dev/null +++ b/src/scenic/simulators/isaac/lab_env.py @@ -0,0 +1,116 @@ +import gymnasium as gym + +from scenic.simulators.isaac.terrain_utils import ( + build_scenic_terrain_data, + terrain_objects_from_scene, +) + + +def _unwrap_env(env): + return getattr(env, "unwrapped", env) + + +def _remove_imported_terrain(terrain, name): + cfg = getattr(terrain, "cfg", None) + prim_path = f"{cfg.prim_path}/{name}" if getattr(cfg, "prim_path", None) else None + + if prim_path is not None: + try: + from isaacsim.core.utils.prims import delete_prim + + delete_prim(prim_path) + except Exception: + pass + + terrain_prim_paths = getattr(terrain, "terrain_prim_paths", None) + if terrain_prim_paths is not None and prim_path is not None: + while prim_path in terrain_prim_paths: + terrain_prim_paths.remove(prim_path) + + for attr in ("meshes", "warp_meshes"): + mapping = getattr(terrain, attr, None) + if isinstance(mapping, dict): + mapping.pop(name, None) + + +def install_scenic_terrain(env, terrain_data): + unwrapped = _unwrap_env(env) + scene = getattr(unwrapped, "scene", None) + terrain = getattr(scene, "terrain", None) if scene is not None else None + if terrain is None and scene is not None: + terrain = getattr(scene, "_terrain", None) + if terrain is None: + raise RuntimeError("Isaac Lab environment does not expose a scene terrain") + + cfg = getattr(terrain, "cfg", None) + _remove_imported_terrain(terrain, "terrain") + + terrain.import_mesh("terrain", terrain_data.terrain_mesh) + terrain.configure_env_origins(terrain_data.terrain_origins) + if hasattr(terrain, "_terrain_flat_patches"): + terrain._terrain_flat_patches = dict(terrain_data.flat_patches) + + if getattr(cfg, "debug_vis", False): + terrain.set_debug_vis(True) + + +def configure_env_cfg_for_scenic_terrain(env_cfg, terrain_data): + from isaaclab.terrains import TerrainGeneratorCfg + from scenic.simulators.isaac.terrain_generator import scenic_terrain_generator_class + + terrain_cfg = env_cfg.scene.terrain + generator_cfg = terrain_cfg.terrain_generator + + num_rows, num_cols = terrain_data.terrain_origins.shape[:2] + + if generator_cfg is None: + generator_cfg = TerrainGeneratorCfg( + size=(1.0, 1.0), + num_rows=int(num_rows), + num_cols=int(num_cols), + sub_terrains={}, # important: avoids missing config field validation + ) + terrain_cfg.terrain_generator = generator_cfg + + terrain_cfg.terrain_type = "generator" + generator_cfg.class_type = scenic_terrain_generator_class(terrain_data) + generator_cfg.num_rows = int(num_rows) + generator_cfg.num_cols = int(num_cols) + generator_cfg.sub_terrains = {} + + if hasattr(env_cfg, "curriculum") and hasattr(env_cfg.curriculum, "terrain_levels"): + env_cfg.curriculum.terrain_levels = None + if hasattr(terrain_cfg, "max_init_terrain_level"): + terrain_cfg.max_init_terrain_level = None + + +class ScenicIsaacLabEnvWrapper(gym.Wrapper): + def __init__(self, env, scenario, *, terrain_border_width=20.0, max_iterations=2000): + super().__init__(env) + self.scenario = scenario + self.terrain_border_width = terrain_border_width + self.max_iterations = max_iterations + self.current_scene = None + self.current_terrain_data = None + + def __getattr__(self, name): + if name.startswith("_"): + raise AttributeError(name) + return getattr(self.env, name) + + def _sample_terrain(self): + scene, _ = self.scenario.generate(maxIterations=self.max_iterations) + terrain_data = build_scenic_terrain_data( + terrain_objects_from_scene(scene), border_width=self.terrain_border_width + ) + self.current_scene = scene + self.current_terrain_data = terrain_data + return terrain_data + + def reset(self, *, seed=None, options=None): + terrain_data = self._sample_terrain() + install_scenic_terrain(self.env, terrain_data) + return self.env.reset(seed=seed, options=options) + + def step(self, action): + return self.env.step(action) diff --git a/src/scenic/simulators/isaac/model.scenic b/src/scenic/simulators/isaac/model.scenic index 23cd4d939..82f0abd12 100644 --- a/src/scenic/simulators/isaac/model.scenic +++ b/src/scenic/simulators/isaac/model.scenic @@ -16,7 +16,8 @@ from scenic.simulators.isaac.actions import ( ) from scenic.simulators.isaac.behaviors import * from scenic.simulators.isaac.backends import DEFAULT_BACKEND_NAME, get_backend, set_default_backend -from scenic.simulators.isaac.simulator import IsaacSimSimulator +from scenic.simulators.isaac import TerrainBase +from scenic.simulators.isaac.simulator import IsaacSimulator from scenic.simulators.isaac.utils import _addExistingObj, EnvironmentMeshCache # ---------- global parameters ---------- @@ -26,18 +27,44 @@ param environmentInfoPath = None param environmentMeshPath = None param headless = False param isaacBackend = DEFAULT_BACKEND_NAME +param isaacLab = False environmentMeshPath = globalParameters.environmentMeshPath environmentInfoPath = globalParameters.environmentInfoPath set_default_backend(globalParameters.isaacBackend) -isaac_backend = get_backend(globalParameters.isaacBackend) +isaac_backend = get_backend("lab" if globalParameters.isaacLab else globalParameters.isaacBackend) + +param labEnvCfg = None +param labDevice = "cuda:0" +param labNumEnvs = 1 +param labEnvSpacing = 10.0 +param labTimestep = 1 / 120 + +param labTask = None +param labDebugLifecycle = False # ---------- simulator creation ---------- -simulator IsaacSimSimulator( - environmentUSDPath=globalParameters.environmentUSDPath, - headless=globalParameters.headless, - backend=globalParameters.isaacBackend, -) +if isaac_backend.name == 'lab': + simulator IsaacSimulator( + environmentUSDPath=globalParameters.environmentUSDPath, + headless=globalParameters.headless, + backend=globalParameters.isaacBackend, + isaacLab=globalParameters.isaacLab, + task = globalParameters.labTask, + env_cfg_entry_point=globalParameters.labEnvCfg, + device=globalParameters.labDevice, + num_envs=globalParameters.labNumEnvs, + env_spacing=globalParameters.labEnvSpacing, + timestep=globalParameters.labTimestep, + debug_lifecycle=globalParameters.labDebugLifecycle, + ) +else: + simulator IsaacSimulator( + environmentUSDPath=globalParameters.environmentUSDPath, + headless=globalParameters.headless, + backend=globalParameters.isaacBackend, + isaacLab=globalParameters.isaacLab, + ) # ---------- base classes ---------- @@ -65,10 +92,12 @@ class ExistingIsaacSimObject(IsaacSimObject): class IsaacSimRobot(IsaacSimObject, _Robot): + name: f"Robot_{uuid.uuid4().hex[:8]}" controller: None control: None usd_path: None isaac_asset_path: None + initial_rotation: None blueprint: "Robot" def create(self): @@ -83,26 +112,36 @@ class Create3(IsaacSimRobot, _WheeledRobot): width: 0.335 length: 0.335 height: .1 - - def create(self): - return isaac_backend.create_create3(self) - - def move(self, sim, throttle=0, steering=0): - sim.backend.apply_wheeled_control(sim, self, [throttle, steering]) + isaac_asset_path: "Isaac/Robots/iRobot/Create3/create_3.usd" + + # Differential-drive metadata. + wheel_radius: 0.03575 + wheel_base: 0.233 + wheel_dof_names: ["left_wheel_joint", "right_wheel_joint"] + wheel_controller: "differential" + +class Jetbot(IsaacSimRobot, _WheeledRobot): + width: 0.16 + length: 0.16 + height: 0.12 + isaac_asset_path: "Isaac/Robots/NVIDIA/Jetbot/jetbot.usd" + + # Differential-drive metadata. + wheel_radius: 0.03 + wheel_base: 0.1125 + wheel_dof_names: ["left_wheel_joint", "right_wheel_joint"] + wheel_controller: "differential" class Kaya(IsaacSimRobot, _HolonomicRobot): width: 0.2 length: 0.2 height: 0.2 + isaac_asset_path: "Isaac/Robots/NVIDIA/Kaya/kaya.usd" - def create(self): - return isaac_backend.create_kaya(self) - - def move(self, sim, forward_speed=0, lateral_speed=0, yaw_speed=0): - sim.backend.apply_wheeled_control( - sim, self, [forward_speed, lateral_speed, yaw_speed] - ) + # Holonomic-drive metadata. + wheel_dof_names: ["axle_0_joint", "axle_1_joint", "axle_2_joint"] + wheel_controller: "holonomic" class FrankaPanda(IsaacSimRobot, _ManipulatorRobot): @@ -115,8 +154,9 @@ class FrankaPanda(IsaacSimRobot, _ManipulatorRobot): width: 0.3 length: 0.3 height: 0.9 - end_effector_offset: np.array([0, 0.005, 0]) + end_effector_offset: [0.0, 0.0, 0.0] end_effector_orientation: None + franka_pick_place_events_dt: [60, 40, 20, 40, 80, 20, 20] def create(self): return isaac_backend.create_franka_panda(self) @@ -139,7 +179,8 @@ class FrankaPanda(IsaacSimRobot, _ManipulatorRobot): ) class GroundPlane(IsaacSimObject): - + + name: "GroundPlane" width: 5 length: 5 height: 0.01 @@ -149,6 +190,198 @@ class GroundPlane(IsaacSimObject): def create(self): return isaac_backend.create_ground_plane(self) +class Terrain: + horizontal_scale: TerrainBase.horizontal_scale + vertical_scale: TerrainBase.vertical_scale + width: 10.0 + length: 10.0 + size: (self.width, self.length) + difficulty: None + blueprint: "Terrain" + physics: False + position: (0, 0, 0) + + # These will be filled by subclasses in create() + mesh: None + subterrain: None + +class RandomUniformTerrain(Terrain): + name: f"RandomUniformTerrain_{uuid.uuid4().hex[:8]}" + + noise_range: (0.0, 1.0) + noise_step: 0.01 + downsampled_scale: 1.0 + + def create(self): + from scenic.core.terrain import random_uniform_terrain, RandomUniformTerrainCfg, subterrain_to_mesh + # Build configuration and register the generator function for the simulator. + terrain_cfg = RandomUniformTerrainCfg( + noise_range=self.noise_range, + noise_step=self.noise_step, + downsampled_scale=self.downsampled_scale, + horizontal_scale=self.horizontal_scale, + vertical_scale=self.vertical_scale, + border_width=1.0, + size=self.size, + ) + subterrain = random_uniform_terrain(cfg=terrain_cfg, difficulty=self.difficulty) + self.mesh = subterrain_to_mesh(subterrain) + self.subterrain = subterrain + +class SlopedTerrain(Terrain): + name: f"SlopedTerrain_{uuid.uuid4().hex[:8]}" + slope: 0.25 + + def create(self): + from scenic.core.terrain import sloped_terrain, SlopedTerrainCfg, subterrain_to_mesh + terrain_cfg = SlopedTerrainCfg( + slope=self.slope, + horizontal_scale=self.horizontal_scale, + vertical_scale=self.vertical_scale, + size=self.size, + ) + subterrain = sloped_terrain(cfg=terrain_cfg, difficulty=self.difficulty) + self.mesh = subterrain_to_mesh(subterrain) + self.subterrain = subterrain + +class PyramidSlopedTerrain(Terrain): + name: f"PyramidSlopedTerrain_{uuid.uuid4().hex[:8]}" + slope: 0.25 + platform_size: 1.0 + + def create(self): + from scenic.core.terrain import pyramid_sloped_terrain, PyramidSlopedTerrainCfg, subterrain_to_mesh + terrain_cfg = PyramidSlopedTerrainCfg( + slope=self.slope, + platform_size=self.platform_size, + horizontal_scale=self.horizontal_scale, + vertical_scale=self.vertical_scale, + size=self.size, + ) + subterrain = pyramid_sloped_terrain(cfg=terrain_cfg, difficulty=self.difficulty) + self.mesh = subterrain_to_mesh(subterrain) + self.subterrain = subterrain + +class DiscreteObstaclesTerrain(Terrain): + name: f"DiscreteObstaclesTerrain_{uuid.uuid4().hex[:8]}" + max_height: 0.2 + min_size: 0.5 + max_size: 2.0 + num_rects: 5 + platform_size: 1.0 + + def create(self): + from scenic.core.terrain import discrete_obstacles_terrain, DiscreteObstaclesTerrainCfg, subterrain_to_mesh + terrain_cfg = DiscreteObstaclesTerrainCfg( + max_height=self.max_height, + min_size=self.min_size, + max_size=self.max_size, + num_rects=self.num_rects, + platform_size=self.platform_size, + horizontal_scale=self.horizontal_scale, + vertical_scale=self.vertical_scale, + size=self.size, + ) + subterrain = discrete_obstacles_terrain(cfg=terrain_cfg, difficulty=self.difficulty) + self.mesh = subterrain_to_mesh(subterrain) + self.subterrain = subterrain + +class WaveTerrain(Terrain): + name: f"WaveTerrain_{uuid.uuid4().hex[:8]}" + num_waves: 1 + amplitude: 1.0 + + def create(self): + from scenic.core.terrain import wave_terrain, WaveTerrainCfg, subterrain_to_mesh + cfg = WaveTerrainCfg( + num_waves=self.num_waves, + amplitude=self.amplitude, + size=self.size, + horizontal_scale=self.horizontal_scale, + vertical_scale=self.vertical_scale, + ) + sub = wave_terrain(cfg=cfg, difficulty=self.difficulty) + self.mesh = subterrain_to_mesh(sub) + self.subterrain = sub + +class StairsTerrain(Terrain): + name: f"StairsTerrain_{uuid.uuid4().hex[:8]}" + step_width: 1.0 + step_height: 0.1 + + def create(self): + from scenic.core.terrain import stairs_terrain, StairsTerrainCfg, subterrain_to_mesh + cfg = StairsTerrainCfg( + step_width=self.step_width, + step_height=self.step_height, + size=self.size, + horizontal_scale=self.horizontal_scale, + vertical_scale=self.vertical_scale, + ) + sub = stairs_terrain(cfg=cfg, difficulty=self.difficulty) + self.mesh = subterrain_to_mesh(sub) + self.subterrain = sub + +class PyramidStairsTerrain(Terrain): + name: f"PyramidStairsTerrain_{uuid.uuid4().hex[:8]}" + step_width: 1.0 + step_height: 0.1 + platform_size: 1.0 + + def create(self): + from scenic.core.terrain import pyramid_stairs_terrain, PyramidStairsTerrainCfg, subterrain_to_mesh + cfg = PyramidStairsTerrainCfg( + step_width=self.step_width, + step_height=self.step_height, + platform_size=self.platform_size, + size=self.size, + horizontal_scale=self.horizontal_scale, + vertical_scale=self.vertical_scale, + ) + sub = pyramid_stairs_terrain(cfg=cfg, difficulty=self.difficulty) + self.mesh = subterrain_to_mesh(sub) + self.subterrain = sub + +class SteppingStonesTerrain(Terrain): + name: f"SteppingStonesTerrain_{uuid.uuid4().hex[:8]}" + stone_size: 1.0 + stone_distance: 1.0 + max_height: 0.2 + platform_size: 1.0 + depth: -10.0 + + def create(self): + from scenic.core.terrain import stepping_stones_terrain, SteppingStonesTerrainCfg, subterrain_to_mesh + cfg = SteppingStonesTerrainCfg( + stone_size=self.stone_size, + stone_distance=self.stone_distance, + max_height=self.max_height, + platform_size=self.platform_size, + depth=self.depth, + size=self.size, + horizontal_scale=self.horizontal_scale, + vertical_scale=self.vertical_scale, + ) + sub = stepping_stones_terrain(cfg=cfg, difficulty=self.difficulty) + self.mesh = subterrain_to_mesh(sub) + self.subterrain = sub + +class PolesTerrain(Terrain): + name: f"PolesTerrain_{uuid.uuid4().hex[:8]}" + difficulty: 1.0 + + def create(self): + from scenic.core.terrain import poles_terrain, PolesTerrainCfg, subterrain_to_mesh + cfg = PolesTerrainCfg( + difficulty=self.difficulty, + size=self.size, + horizontal_scale=self.horizontal_scale, + vertical_scale=self.vertical_scale, + ) + sub = poles_terrain(cfg=cfg, difficulty=self.difficulty) + self.mesh = subterrain_to_mesh(sub) + self.subterrain = sub + if globalParameters.environmentUSDPath: try: @@ -220,7 +453,7 @@ if globalParameters.environmentUSDPath: world_center = np.dot(world_transform, local_center_homogeneous)[:3] if "usd_dimensions" in info: - dimensions = tuple(float(x) for x in info["usd_dimensions"]) + dimensions = tuple(np.maximum(float(x), 1e-6) for x in info["usd_dimensions"]) else: scale_vec = np.abs(np.array(scale, dtype=float)) raw_extents = np.array(mesh.extents, dtype=float) diff --git a/src/scenic/simulators/isaac/scripts/__init__.py b/src/scenic/simulators/isaac/scripts/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/scenic/simulators/isaac/scripts/cli_args.py b/src/scenic/simulators/isaac/scripts/cli_args.py new file mode 100644 index 000000000..3cf6f0f71 --- /dev/null +++ b/src/scenic/simulators/isaac/scripts/cli_args.py @@ -0,0 +1,30 @@ +def add_rsl_rl_args(parser): + group = parser.add_argument_group("rsl_rl") + group.add_argument("--experiment_name", type=str, default=None) + group.add_argument("--run_name", type=str, default=None) + group.add_argument("--resume", action="store_true", default=False) + group.add_argument("--load_run", type=str, default=None) + group.add_argument("--checkpoint", type=str, default=None) + group.add_argument("--logger", type=str, default=None) + group.add_argument("--log_project_name", type=str, default=None) + + +def update_rsl_rl_cfg(agent_cfg, args_cli): + if args_cli.seed is not None: + agent_cfg.seed = args_cli.seed + if args_cli.resume: + agent_cfg.resume = args_cli.resume + if args_cli.load_run is not None: + agent_cfg.load_run = args_cli.load_run + if args_cli.checkpoint is not None: + agent_cfg.load_checkpoint = args_cli.checkpoint + if args_cli.run_name is not None: + agent_cfg.run_name = args_cli.run_name + if args_cli.experiment_name is not None: + agent_cfg.experiment_name = args_cli.experiment_name + if args_cli.logger is not None: + agent_cfg.logger = args_cli.logger + if args_cli.log_project_name is not None: + agent_cfg.wandb_project = args_cli.log_project_name + agent_cfg.neptune_project = args_cli.log_project_name + return agent_cfg diff --git a/src/scenic/simulators/isaac/scripts/common.py b/src/scenic/simulators/isaac/scripts/common.py new file mode 100644 index 000000000..0ceae9853 --- /dev/null +++ b/src/scenic/simulators/isaac/scripts/common.py @@ -0,0 +1,29 @@ +import scenic + +from scenic.simulators.isaac.lab_env import ( + ScenicIsaacLabEnvWrapper, + configure_env_cfg_for_scenic_terrain, +) +from scenic.simulators.isaac.terrain_utils import ( + build_scenic_terrain_data, + terrain_objects_from_scene, +) + + +def load_scenic_scenario(path, model): + return scenic.scenarioFromFile(path, model=model, params={"isaacLab": True}) + + +def configure_initial_scenic_terrain(env_cfg, scenario, terrain_border_width): + scene, _ = scenario.generate() + terrain_data = build_scenic_terrain_data( + terrain_objects_from_scene(scene), border_width=terrain_border_width + ) + configure_env_cfg_for_scenic_terrain(env_cfg, terrain_data) + return scene, terrain_data + + +def wrap_with_scenic(env, scenario, terrain_border_width): + return ScenicIsaacLabEnvWrapper( + env, scenario, terrain_border_width=terrain_border_width + ) diff --git a/src/scenic/simulators/isaac/scripts/play_scenic.py b/src/scenic/simulators/isaac/scripts/play_scenic.py new file mode 100644 index 000000000..a8aa39d62 --- /dev/null +++ b/src/scenic/simulators/isaac/scripts/play_scenic.py @@ -0,0 +1,185 @@ +import argparse +import os +import sys +import time +from pathlib import Path + +# _SCENIC_SRC = Path(__file__).resolve().parents[4] +# if str(_SCENIC_SRC) not in sys.path: +# sys.path.insert(0, str(_SCENIC_SRC)) + +from isaaclab.app import AppLauncher + +from scenic.simulators.isaac.scripts import cli_args + +parser = argparse.ArgumentParser(description="Play an RSL-RL checkpoint in Scenic Isaac Lab terrain.") +parser.add_argument("--video", action="store_true", default=False) +parser.add_argument("--video_length", type=int, default=200) +parser.add_argument("--disable_fabric", action="store_true", default=False) +parser.add_argument("--num_envs", type=int, default=None) +parser.add_argument("--task", type=str, default=None) +parser.add_argument("--agent", type=str, default="rsl_rl_cfg_entry_point") +parser.add_argument("--seed", type=int, default=None) +parser.add_argument("--use_pretrained_checkpoint", action="store_true") +parser.add_argument("--real-time", action="store_true", default=False) +parser.add_argument( + "--scenario", + type=str, + default="examples/isaacsim/terrain/random_uniform.scenic", +) +parser.add_argument("--scenic_model", type=str, default="scenic.simulators.isaac.model") +parser.add_argument("--terrain_border_width", type=float, default=20.0) + +cli_args.add_rsl_rl_args(parser) +AppLauncher.add_app_launcher_args(parser) +args_cli, hydra_args = parser.parse_known_args() + +if args_cli.video: + args_cli.enable_cameras = True + +sys.argv = [sys.argv[0]] + hydra_args + +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +import importlib.metadata as metadata + +import gymnasium as gym +import torch +from packaging import version +from rsl_rl.runners import DistillationRunner, OnPolicyRunner + +from isaaclab.envs import ( + DirectMARLEnv, + DirectMARLEnvCfg, + DirectRLEnvCfg, + ManagerBasedRLEnvCfg, + multi_agent_to_single_agent, +) +from isaaclab.utils.assets import retrieve_file_path +from isaaclab.utils.dict import print_dict +from isaaclab_rl.rsl_rl import ( + RslRlBaseRunnerCfg, + RslRlVecEnvWrapper, + export_policy_as_jit, + export_policy_as_onnx, + handle_deprecated_rsl_rl_cfg, +) +from isaaclab_rl.utils.pretrained_checkpoint import get_published_pretrained_checkpoint + +import isaaclab_tasks # noqa: F401 +from isaaclab_tasks.utils import get_checkpoint_path +from isaaclab_tasks.utils.hydra import hydra_task_config + +from scenic.simulators.isaac.scripts.common import ( + configure_initial_scenic_terrain, + load_scenic_scenario, + wrap_with_scenic, +) + +installed_version = metadata.version("rsl-rl-lib") + + +@hydra_task_config(args_cli.task, args_cli.agent) +def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agent_cfg: RslRlBaseRunnerCfg): + task_name = args_cli.task.split(":")[-1] + train_task_name = task_name.replace("-Play", "") + + agent_cfg = cli_args.update_rsl_rl_cfg(agent_cfg, args_cli) + env_cfg.scene.num_envs = args_cli.num_envs if args_cli.num_envs is not None else env_cfg.scene.num_envs + agent_cfg = handle_deprecated_rsl_rl_cfg(agent_cfg, installed_version) + + env_cfg.seed = agent_cfg.seed + env_cfg.sim.device = args_cli.device if args_cli.device is not None else env_cfg.sim.device + if hasattr(env_cfg, "episode_length_s"): + env_cfg.episode_length_s = 10.0 + + scenario = load_scenic_scenario(args_cli.scenario, args_cli.scenic_model) + configure_initial_scenic_terrain(env_cfg, scenario, args_cli.terrain_border_width) + + log_root_path = os.path.abspath(os.path.join("logs", "rsl_rl", agent_cfg.experiment_name)) + print(f"[INFO] Loading experiment from directory: {log_root_path}") + if args_cli.use_pretrained_checkpoint: + resume_path = get_published_pretrained_checkpoint("rsl_rl", train_task_name) + if not resume_path: + print("[INFO] No published pre-trained checkpoint is available for this task.") + return + elif args_cli.checkpoint: + resume_path = retrieve_file_path(args_cli.checkpoint) + else: + resume_path = get_checkpoint_path(log_root_path, agent_cfg.load_run, agent_cfg.load_checkpoint) + + log_dir = os.path.dirname(resume_path) + env_cfg.log_dir = log_dir + + env = gym.make(args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None) + if isinstance(env.unwrapped, DirectMARLEnv): + env = multi_agent_to_single_agent(env) + + if args_cli.video: + video_kwargs = { + "video_folder": os.path.join(log_dir, "videos", "play"), + "step_trigger": lambda step: step == 0, + "video_length": args_cli.video_length, + "disable_logger": True, + } + print("[INFO] Recording videos during play.") + print_dict(video_kwargs, nesting=4) + env = gym.wrappers.RecordVideo(env, **video_kwargs) + + env = wrap_with_scenic(env, scenario, args_cli.terrain_border_width) + env = RslRlVecEnvWrapper(env, clip_actions=agent_cfg.clip_actions) + + print(f"[INFO]: Loading model checkpoint from: {resume_path}") + if agent_cfg.class_name == "OnPolicyRunner": + runner = OnPolicyRunner(env, agent_cfg.to_dict(), log_dir=None, device=agent_cfg.device) + elif agent_cfg.class_name == "DistillationRunner": + runner = DistillationRunner(env, agent_cfg.to_dict(), log_dir=None, device=agent_cfg.device) + else: + raise ValueError(f"Unsupported runner class: {agent_cfg.class_name}") + runner.load(resume_path) + + policy = runner.get_inference_policy(device=env.unwrapped.device) + export_model_dir = os.path.join(os.path.dirname(resume_path), "exported") + + policy_nn = None + if version.parse(installed_version) >= version.parse("4.0.0"): + runner.export_policy_to_jit(path=export_model_dir, filename="policy.pt") + runner.export_policy_to_onnx(path=export_model_dir, filename="policy.onnx") + else: + policy_nn = runner.alg.policy if version.parse(installed_version) >= version.parse("2.3.0") else runner.alg.actor_critic + normalizer = getattr(policy_nn, "actor_obs_normalizer", None) + if normalizer is None: + normalizer = getattr(policy_nn, "student_obs_normalizer", None) + export_policy_as_jit(policy_nn, normalizer=normalizer, path=export_model_dir, filename="policy.pt") + export_policy_as_onnx(policy_nn, normalizer=normalizer, path=export_model_dir, filename="policy.onnx") + + dt = env.unwrapped.step_dt + obs = env.get_observations() + timestep = 0 + while simulation_app.is_running(): + start_time = time.time() + with torch.inference_mode(): + actions = policy(obs) + obs, _, dones, _ = env.step(actions) + if version.parse(installed_version) >= version.parse("4.0.0"): + if hasattr(policy, "reset"): + policy.reset(dones) + elif policy_nn is not None and hasattr(policy_nn, "reset"): + policy_nn.reset(dones) + + if args_cli.video: + timestep += 1 + if timestep == args_cli.video_length: + break + + sleep_time = dt - (time.time() - start_time) + if args_cli.real_time and sleep_time > 0: + time.sleep(sleep_time) + + env.close() + + +if __name__ == "__main__": + main() + simulation_app.close() diff --git a/src/scenic/simulators/isaac/scripts/train_scenic.py b/src/scenic/simulators/isaac/scripts/train_scenic.py new file mode 100644 index 000000000..121e5a45d --- /dev/null +++ b/src/scenic/simulators/isaac/scripts/train_scenic.py @@ -0,0 +1,181 @@ +import argparse +import logging +import os +import sys +import time +from datetime import datetime +from pathlib import Path + +# _SCENIC_SRC = Path(__file__).resolve().parents[4] +# if str(_SCENIC_SRC) not in sys.path: +# sys.path.insert(0, str(_SCENIC_SRC)) + +from isaaclab.app import AppLauncher + +from scenic.simulators.isaac.scripts import cli_args + +parser = argparse.ArgumentParser(description="Train an RSL-RL agent in Scenic Isaac Lab terrain.") +parser.add_argument("--video", action="store_true", default=False) +parser.add_argument("--video_length", type=int, default=200) +parser.add_argument("--video_interval", type=int, default=2000) +parser.add_argument("--num_envs", type=int, default=None) +parser.add_argument("--task", type=str, default=None) +parser.add_argument("--agent", type=str, default="rsl_rl_cfg_entry_point") +parser.add_argument("--seed", type=int, default=None) +parser.add_argument("--max_iterations", type=int, default=None) +parser.add_argument("--distributed", action="store_true", default=False) +parser.add_argument("--export_io_descriptors", action="store_true", default=False) +parser.add_argument("--ray-proc-id", "-rid", type=int, default=None) +parser.add_argument( + "--scenario", + type=str, + default="examples/isaacsim/terrain/random_uniform.scenic", +) +parser.add_argument("--scenic_model", type=str, default="scenic.simulators.isaac.model") +parser.add_argument("--terrain_border_width", type=float, default=20.0) + +cli_args.add_rsl_rl_args(parser) +AppLauncher.add_app_launcher_args(parser) +args_cli, hydra_args = parser.parse_known_args() + +if args_cli.video: + args_cli.enable_cameras = True + +sys.argv = [sys.argv[0]] + hydra_args + +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +import importlib.metadata as metadata +import platform + +import gymnasium as gym +import torch +from packaging import version +from rsl_rl.runners import DistillationRunner, OnPolicyRunner + +from isaaclab.envs import ( + DirectMARLEnv, + DirectMARLEnvCfg, + DirectRLEnvCfg, + ManagerBasedRLEnvCfg, + multi_agent_to_single_agent, +) +from isaaclab.utils.dict import print_dict +from isaaclab.utils.io import dump_yaml +from isaaclab_rl.rsl_rl import RslRlBaseRunnerCfg, RslRlVecEnvWrapper, handle_deprecated_rsl_rl_cfg + +import isaaclab_tasks # noqa: F401 +from isaaclab_tasks.utils import get_checkpoint_path +from isaaclab_tasks.utils.hydra import hydra_task_config + +from scenic.simulators.isaac.scripts.common import ( + configure_initial_scenic_terrain, + load_scenic_scenario, + wrap_with_scenic, +) + +logger = logging.getLogger(__name__) + +RSL_RL_VERSION = "3.0.1" +installed_version = metadata.version("rsl-rl-lib") +if version.parse(installed_version) < version.parse(RSL_RL_VERSION): + command = ( + [r".\isaaclab.bat", "-p", "-m", "pip", "install", f"rsl-rl-lib=={RSL_RL_VERSION}"] + if platform.system() == "Windows" + else ["./isaaclab.sh", "-p", "-m", "pip", "install", f"rsl-rl-lib=={RSL_RL_VERSION}"] + ) + raise RuntimeError( + f"rsl-rl-lib {RSL_RL_VERSION} or newer is required. " + f"Installed: {installed_version}. Install with: {' '.join(command)}" + ) + +torch.backends.cuda.matmul.allow_tf32 = True +torch.backends.cudnn.allow_tf32 = True +torch.backends.cudnn.deterministic = False +torch.backends.cudnn.benchmark = False + + +@hydra_task_config(args_cli.task, args_cli.agent) +def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agent_cfg: RslRlBaseRunnerCfg): + agent_cfg = cli_args.update_rsl_rl_cfg(agent_cfg, args_cli) + env_cfg.scene.num_envs = args_cli.num_envs if args_cli.num_envs is not None else env_cfg.scene.num_envs + agent_cfg.max_iterations = ( + args_cli.max_iterations if args_cli.max_iterations is not None else agent_cfg.max_iterations + ) + agent_cfg = handle_deprecated_rsl_rl_cfg(agent_cfg, installed_version) + + env_cfg.seed = agent_cfg.seed + env_cfg.sim.device = args_cli.device if args_cli.device is not None else env_cfg.sim.device + if args_cli.distributed and args_cli.device is not None and "cpu" in args_cli.device: + raise ValueError("Distributed training requires a GPU device.") + + if args_cli.distributed: + env_cfg.sim.device = f"cuda:{app_launcher.local_rank}" + agent_cfg.device = f"cuda:{app_launcher.local_rank}" + seed = agent_cfg.seed + app_launcher.local_rank + env_cfg.seed = seed + agent_cfg.seed = seed + + scenario = load_scenic_scenario(args_cli.scenario, args_cli.scenic_model) + configure_initial_scenic_terrain(env_cfg, scenario, args_cli.terrain_border_width) + + log_root_path = os.path.abspath(os.path.join("logs", "rsl_rl", agent_cfg.experiment_name)) + print(f"[INFO] Logging experiment in directory: {log_root_path}") + log_dir = datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + print(f"Exact experiment name requested from command line: {log_dir}") + if agent_cfg.run_name: + log_dir += f"_{agent_cfg.run_name}" + log_dir = os.path.join(log_root_path, log_dir) + + if isinstance(env_cfg, ManagerBasedRLEnvCfg): + env_cfg.export_io_descriptors = args_cli.export_io_descriptors + else: + logger.warning("IO descriptors are only supported for manager based RL environments.") + env_cfg.log_dir = log_dir + + env = gym.make(args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None) + if isinstance(env.unwrapped, DirectMARLEnv): + env = multi_agent_to_single_agent(env) + + if agent_cfg.resume or agent_cfg.algorithm.class_name == "Distillation": + resume_path = get_checkpoint_path(log_root_path, agent_cfg.load_run, agent_cfg.load_checkpoint) + + if args_cli.video: + video_kwargs = { + "video_folder": os.path.join(log_dir, "videos", "train"), + "step_trigger": lambda step: step % args_cli.video_interval == 0, + "video_length": args_cli.video_length, + "disable_logger": True, + } + print("[INFO] Recording videos during training.") + print_dict(video_kwargs, nesting=4) + env = gym.wrappers.RecordVideo(env, **video_kwargs) + + env = wrap_with_scenic(env, scenario, args_cli.terrain_border_width) + start_time = time.time() + env = RslRlVecEnvWrapper(env, clip_actions=agent_cfg.clip_actions) + + if agent_cfg.class_name == "OnPolicyRunner": + runner = OnPolicyRunner(env, agent_cfg.to_dict(), log_dir=log_dir, device=agent_cfg.device) + elif agent_cfg.class_name == "DistillationRunner": + runner = DistillationRunner(env, agent_cfg.to_dict(), log_dir=log_dir, device=agent_cfg.device) + else: + raise ValueError(f"Unsupported runner class: {agent_cfg.class_name}") + + runner.add_git_repo_to_log(__file__) + if agent_cfg.resume or agent_cfg.algorithm.class_name == "Distillation": + print(f"[INFO]: Loading model checkpoint from: {resume_path}") + runner.load(resume_path) + + dump_yaml(os.path.join(log_dir, "params", "env.yaml"), env_cfg) + dump_yaml(os.path.join(log_dir, "params", "agent.yaml"), agent_cfg) + + runner.learn(num_learning_iterations=agent_cfg.max_iterations, init_at_random_ep_len=True) + print(f"Training time: {round(time.time() - start_time, 2)} seconds") + env.close() + + +if __name__ == "__main__": + main() + simulation_app.close() diff --git a/src/scenic/simulators/isaac/simulator.py b/src/scenic/simulators/isaac/simulator.py index efe1fcec9..7bbef0b52 100644 --- a/src/scenic/simulators/isaac/simulator.py +++ b/src/scenic/simulators/isaac/simulator.py @@ -11,6 +11,29 @@ from scenic.simulators.isaac.backends import get_backend import scenic.simulators.isaac.utils as utils + +class IsaacSimulator(Simulator): + def __init__(self, isaacLab=False, **kwargs): + super().__init__() + self.isaacLab = isaacLab + if isaacLab: + from scenic.simulators.isaac.lab import IsaacLabSimulator + + self.delegate = IsaacLabSimulator(**kwargs) + else: + self.delegate = IsaacSimSimulator(**kwargs) + + def createSimulation(self, scene, **kwargs): + return self.delegate.createSimulation(scene, **kwargs) + + def simulate(self, scene, *args, **kwargs): + return self.delegate.simulate(scene, *args, **kwargs) + + def destroy(self): + super().destroy() + self.delegate.destroy() + + class IsaacSimSimulator(Simulator): def __init__(self, headless=False, environmentUSDPath=None, backend=None): super().__init__() @@ -31,6 +54,9 @@ def createSimulation(self, scene, **kwargs): ) def destroy(self): + print("[IsaacSimSimulator.destroy] CLOSING SIMULATION APP", flush=True) + import traceback + traceback.print_stack(limit=20) super().destroy() self.backend.close_simulation_app(self.client) @@ -117,15 +143,12 @@ def step(self): self.backend.step_world(self.world) def createObjectInSimulator(self, obj): - if obj.blueprint == "IsaacSimObject" and not obj.usd_path: + if obj.blueprint == "IsaacSimObject" and not obj.usd_path and not obj.isaac_asset_path: objectScaledMesh = MeshVolumeRegion( mesh=obj.shape.mesh, dimensions=(obj.width, obj.length, obj.height), ).mesh - if self.backend.name == "core_51": - objectObjMesh = utils.mesh_to_obj_frame(objectScaledMesh) - else: - objectObjMesh = objectScaledMesh + objectObjMesh = utils.mesh_to_obj_frame(objectScaledMesh) obj_file_path = os.path.join(self.tmpMeshDir, f"{obj.name}.obj") usd_file_path = os.path.join(self.tmpMeshDir, f"{obj.name}.usd") trimesh.exchange.export.export_mesh(objectObjMesh, obj_file_path) @@ -143,7 +166,7 @@ def createObjectInSimulator(self, obj): return try: - self.backend.add_object(self.world, isaac_sim_obj) + self.backend.add_object(self.world, isaac_sim_obj, scenic_obj=obj) except Exception as exc: raise SimulationCreationError(f"Unable to add {obj.name} to world") from exc @@ -162,7 +185,7 @@ def getProperties(self, obj, properties): pitch=raw["pitch"], roll=raw["roll"], ) - + def destroy(self): if self.world is not None: self.backend.stop_and_clear_world(self.world) diff --git a/src/scenic/simulators/isaac/terrain_generator.py b/src/scenic/simulators/isaac/terrain_generator.py new file mode 100644 index 000000000..072742b1a --- /dev/null +++ b/src/scenic/simulators/isaac/terrain_generator.py @@ -0,0 +1,21 @@ +from scenic.simulators.isaac.terrain_utils import ScenicTerrainData + +from isaaclab.terrains.terrain_generator import TerrainGenerator + + +class ScenicTerrainGenerator(TerrainGenerator): + def __init__(self, cfg, device="cpu", *, terrain_data: ScenicTerrainData): + self.cfg = cfg + self.device = device + self.terrain_mesh = terrain_data.terrain_mesh.copy() + self.terrain_meshes = [self.terrain_mesh] + self.terrain_origins = terrain_data.terrain_origins.copy() + self.flat_patches = dict(terrain_data.flat_patches) + + +def scenic_terrain_generator_class(terrain_data): + class _ScenicTerrainGenerator(ScenicTerrainGenerator): + def __init__(self, cfg, device="cpu"): + super().__init__(cfg, device=device, terrain_data=terrain_data) + + return _ScenicTerrainGenerator diff --git a/src/scenic/simulators/isaac/terrain_utils.py b/src/scenic/simulators/isaac/terrain_utils.py new file mode 100644 index 000000000..b82ce2699 --- /dev/null +++ b/src/scenic/simulators/isaac/terrain_utils.py @@ -0,0 +1,130 @@ +from dataclasses import dataclass, field + +import numpy as np +import trimesh + + +@dataclass(frozen=True) +class ScenicTerrainData: + terrain_mesh: trimesh.Trimesh + terrain_origins: np.ndarray + flat_patches: dict = field(default_factory=dict) + + +def heightfield_to_trimesh(heightfield, horizontal_scale, vertical_scale): + rows, cols = heightfield.shape + xs = np.arange(rows, dtype=np.float32) * horizontal_scale + ys = np.arange(cols, dtype=np.float32) * horizontal_scale + grid_x, grid_y = np.meshgrid(xs, ys, indexing="ij") + vertices = np.column_stack( + ( + grid_x.ravel(), + grid_y.ravel(), + heightfield.astype(np.float32).ravel() * vertical_scale, + ) + ) + + faces = [] + for i in range(rows - 1): + for j in range(cols - 1): + a = i * cols + j + b = (i + 1) * cols + j + c = i * cols + j + 1 + d = (i + 1) * cols + j + 1 + faces.append((a, b, c)) + faces.append((c, b, d)) + + faces = np.asarray(faces, dtype=np.int64).reshape((-1, 3)) + return trimesh.Trimesh(vertices=vertices, faces=faces, process=False) + + +def build_scenic_terrain_data(terrains, *, border_width=20.0): + terrains = list(terrains) + if not terrains: + raise ValueError("Isaac Lab mode requires at least one Scenic Terrain object") + + horizontal_scale = float(terrains[0].horizontal_scale) + vertical_scale = float(terrains[0].vertical_scale) + border_cells = int(round(border_width / horizontal_scale)) + + terrain_bounds = [] + x_centers = [] + y_centers = [] + min_x = min_y = 0 + max_x = max_y = 0 + + for terrain in terrains: + if terrain.subterrain is None: + terrain.create() + + start_x = int(round(float(terrain.position[0]) / horizontal_scale)) + start_y = int(round(float(terrain.position[1]) / horizontal_scale)) + width_cells = int(round(float(terrain.width) / horizontal_scale)) + length_cells = int(round(float(terrain.length) / horizontal_scale)) + end_x = start_x + width_cells + end_y = start_y + length_cells + + min_x = min(min_x, start_x) + max_x = max(max_x, end_x) + min_y = min(min_y, start_y) + max_y = max(max_y, end_y) + x_centers.append((start_x + end_x) // 2) + y_centers.append((start_y + end_y) // 2) + terrain_bounds.append((terrain, start_x, end_x, start_y, end_y)) + + x_origin_cells = sorted(set(x_centers)) + y_origin_cells = sorted(set(y_centers)) + terrain_origins = np.zeros( + (len(x_origin_cells), len(y_origin_cells), 3), dtype=np.float32 + ) + + heightfield = np.zeros( + ( + max_x - min_x + 2 * border_cells, + max_y - min_y + 2 * border_cells, + ), + dtype=np.float32, + ) + + for terrain, start_x, end_x, start_y, end_y in terrain_bounds: + target_x0 = start_x - min_x + border_cells + target_x1 = end_x - min_x + border_cells + target_y0 = start_y - min_y + border_cells + target_y1 = end_y - min_y + border_cells + heightfield[target_x0:target_x1, target_y0:target_y1] = ( + terrain.subterrain.height_field_raw + ) + + x_center_offset = -0.5 * (min_x + max_x) * horizontal_scale + y_center_offset = -0.5 * (min_y + max_y) * horizontal_scale + + x_index = {value: index for index, value in enumerate(x_origin_cells)} + y_index = {value: index for index, value in enumerate(y_origin_cells)} + for x_pos in x_origin_cells: + for y_pos in y_origin_cells: + height_x = x_pos - min_x + border_cells + height_y = y_pos - min_y + border_cells + terrain_origins[x_index[x_pos], y_index[y_pos], 0] = ( + x_pos * horizontal_scale + x_center_offset + ) + terrain_origins[x_index[x_pos], y_index[y_pos], 1] = ( + y_pos * horizontal_scale + y_center_offset + ) + terrain_origins[x_index[x_pos], y_index[y_pos], 2] = ( + heightfield[height_x, height_y] * vertical_scale + ) + + mesh = heightfield_to_trimesh(heightfield, horizontal_scale, vertical_scale) + mesh.apply_translation( + ( + min_x * horizontal_scale + x_center_offset - border_width, + min_y * horizontal_scale + y_center_offset - border_width, + 0, + ) + ) + + return ScenicTerrainData(terrain_mesh=mesh, terrain_origins=terrain_origins) + + +def terrain_objects_from_scene(scene): + return [obj for obj in scene.objects if getattr(obj, "blueprint", None) == "Terrain"]