Your First Simulation

In this tutorial, you will create a standalone Python application to perform a simulation using 1 vehicle and PX4-Autopilot. The end result should be the equivalent to the Run in Extension Mode (GUI) tutorial. To achieve this, we will cover the basics of the Pegasus Simulator API Reference.

0. Preparation

Before you proceed, make sure you have read the Installing the Pegasus Simulator section first and that your system is correctly configured. Also, we will assume that you already followed the Create a Standalone Application tutorial, as in this tutorial we will resort to concepts introduced in it.

Additionally, you will also be required to have QGroundControl installed to operate the simulated vehicle. If you haven’t already, follow the Preparation section in the Run in Extension Mode (GUI) tutorial.

1. Code

The tutorial corresponds to the 1_px4_single_vehicle.py example in the examples directory.

  1#!/usr/bin/env python
  2"""
  3| File: 1_px4_single_vehicle.py
  4| Author: Marcelo Jacinto (marcelo.jacinto@tecnico.ulisboa.pt)
  5| License: BSD-3-Clause. Copyright (c) 2023, Marcelo Jacinto. All rights reserved.
  6| Description: This files serves as an example on how to build an app that makes use of the Pegasus API to run a simulation with a single vehicle, controlled using the MAVLink control backend.
  7"""
  8
  9# Imports to start Isaac Sim from this script
 10import carb
 11from isaacsim import SimulationApp
 12
 13# Start Isaac Sim's simulation environment
 14# Note: this simulation app must be instantiated right after the SimulationApp import, otherwise the simulator will crash
 15# as this is the object that will load all the extensions and load the actual simulator.
 16simulation_app = SimulationApp({"headless": False})
 17
 18# -----------------------------------
 19# The actual script should start here
 20# -----------------------------------
 21import omni.timeline
 22from omni.isaac.core.world import World
 23
 24# Import the Pegasus API for simulating drones
 25from pegasus.simulator.params import ROBOTS, SIMULATION_ENVIRONMENTS
 26from pegasus.simulator.logic.state import State
 27from pegasus.simulator.logic.backends.px4_mavlink_backend import PX4MavlinkBackend, PX4MavlinkBackendConfig
 28from pegasus.simulator.logic.vehicles.multirotor import Multirotor, MultirotorConfig
 29from pegasus.simulator.logic.interface.pegasus_interface import PegasusInterface
 30# Auxiliary scipy and numpy modules
 31import os.path
 32from scipy.spatial.transform import Rotation
 33
 34class PegasusApp:
 35    """
 36    A Template class that serves as an example on how to build a simple Isaac Sim standalone App.
 37    """
 38
 39    def __init__(self):
 40        """
 41        Method that initializes the PegasusApp and is used to setup the simulation environment.
 42        """
 43
 44        # Acquire the timeline that will be used to start/stop the simulation
 45        self.timeline = omni.timeline.get_timeline_interface()
 46
 47        # Start the Pegasus Interface
 48        self.pg = PegasusInterface()
 49
 50        # Acquire the World, .i.e, the singleton that controls that is a one stop shop for setting up physics, 
 51        # spawning asset primitives, etc.
 52        self.pg._world = World(**self.pg._world_settings)
 53        self.world = self.pg.world
 54
 55        # Launch one of the worlds provided by NVIDIA
 56        self.pg.load_environment(SIMULATION_ENVIRONMENTS["Curved Gridroom"])
 57
 58        # Create the vehicle
 59        # Try to spawn the selected robot in the world to the specified namespace
 60        config_multirotor = MultirotorConfig()
 61        # Create the multirotor configuration
 62        mavlink_config = PX4MavlinkBackendConfig({
 63            "vehicle_id": 0,
 64            "px4_autolaunch": True,
 65            "px4_dir": self.pg.px4_path,
 66            "px4_vehicle_model": self.pg.px4_default_airframe # CHANGE this line to 'iris' if using PX4 version bellow v1.14
 67        })
 68        config_multirotor.backends = [PX4MavlinkBackend(mavlink_config)]
 69
 70        Multirotor(
 71            "/World/quadrotor",
 72            ROBOTS['Iris'],
 73            0,
 74            [0.0, 0.0, 0.07],
 75            Rotation.from_euler("XYZ", [0.0, 0.0, 0.0], degrees=True).as_quat(),
 76            config=config_multirotor,
 77        )
 78
 79        # Reset the simulation environment so that all articulations (aka robots) are initialized
 80        self.world.reset()
 81
 82        # Auxiliar variable for the timeline callback example
 83        self.stop_sim = False
 84
 85    def run(self):
 86        """
 87        Method that implements the application main loop, where the physics steps are executed.
 88        """
 89
 90        # Start the simulation
 91        self.timeline.play()
 92
 93        # The "infinite" loop
 94        while simulation_app.is_running() and not self.stop_sim:
 95
 96            # Update the UI of the app and perform the physics step
 97            self.world.step(render=True)
 98        
 99        # Cleanup and stop
100        carb.log_warn("PegasusApp Simulation App is closing.")
101        self.timeline.stop()
102        simulation_app.close()
103
104def main():
105
106    # Instantiate the template app
107    pg_app = PegasusApp()
108
109    # Run the application loop
110    pg_app.run()
111
112if __name__ == "__main__":
113    main()

2. Explanation

In order to start using the functionalities provided by the Pegasus Simulator, we import some of the most commonly used classes from the pegasus.simulator.logic module, which contains all the core APIs. To check all the classes and methods currently provided, please refer to the API Reference section.

24# Import the Pegasus API for simulating drones
25from pegasus.simulator.params import ROBOTS, SIMULATION_ENVIRONMENTS
26from pegasus.simulator.logic.state import State
27from pegasus.simulator.logic.backends.px4_mavlink_backend import PX4MavlinkBackend, PX4MavlinkBackendConfig
28from pegasus.simulator.logic.vehicles.multirotor import Multirotor, MultirotorConfig
29from pegasus.simulator.logic.interface.pegasus_interface import PegasusInterface

Note

Some APIs may not be documented in the API Reference section. This means one of three things: i) they are not meant for public use; ii) under development (untested); iii) or just deprecated and about to be replaced.

Next, we initialize the PegasusInterface object. This is a singleton, which means that there exists only one PegasusInterface in memory at any time. This interface creates a World class environment. The World class inherits the Simulation Context internally, so you do not need to declare your own manually when using this API. By default, when physics engine is set to run at 250 Hz and the render engine at 60Hz.

The PegasusInterface provides methods to set the simulation environment, define the geographic coordinates of the origin of the world, setting the default path for the PX4-Autopilot installation and much more. To learn more about this class, refer to the Pegasus Interface API section.

47        # Start the Pegasus Interface
48        self.pg = PegasusInterface()

Next, we load the 3D world environment. For this purpose, we provide the auxiliary method load_environment(usd_file) for loading pre-made environments stored in Universal Scene Description (.usd) files. In this example, we load one of the simulation environments already provided by NVIDIA, whose path in stored in the SIMULATION_ENVIRONMENTS dictionary. To known all provided environments, check the Params API section.

55        # Launch one of the worlds provided by NVIDIA
56        self.pg.load_environment(SIMULATION_ENVIRONMENTS["Curved Gridroom"])

The next step is to create a multirotor vehicle object. For that we start by creating a MultirotorConfig, which contains by default a QuadraticThrustCurve, Linear Drag model, a list of Sensors composed of a GPS, IMU, Magnetometer and Barometer, and a list of control Backend.

In this tutorial, we whish to control the vehicle via MAVLink, PX4-Autopilot and QGroundControl. Therefore, we create a MAVLink control backend configured to also launch PX4-Autopilot in SITL mode in the background.

58        # Create the vehicle
59        # Try to spawn the selected robot in the world to the specified namespace
60        config_multirotor = MultirotorConfig()
61        # Create the multirotor configuration
62        mavlink_config = PX4MavlinkBackendConfig({
63            "vehicle_id": 0,
64            "px4_autolaunch": True,
65            "px4_dir": self.pg.px4_path,
66            "px4_vehicle_model": self.pg.px4_default_airframe # CHANGE this line to 'iris' if using PX4 version bellow v1.14
67        })
68        config_multirotor.backends = [PX4MavlinkBackend(mavlink_config)]

To create the actual Multirotor object, we must specify the stage_path, i.e. the name that the vehicle will have inside the simulation tree and the usd_path which defines where the 3D model of the vehicle is stored. Once again, to known all provided vehicle models, check the Params API section. Additionally, we can provide the original position and orientation of the vehicle, according to an East-North-DOWN (ENU) convention.

70        Multirotor(
71            "/World/quadrotor",
72            ROBOTS['Iris'],
73            0,
74            [0.0, 0.0, 0.07],
75            Rotation.from_euler("XYZ", [0.0, 0.0, 0.0], degrees=True).as_quat(),
76            config=config_multirotor,
77        )

After the simulation is setup, the world.reset() method should be invoked to initialize the physics context and set any existing robot joints (the multirotor rotor joints) to their default state.

79        # Reset the simulation environment so that all articulations (aka robots) are initialized
80        self.world.reset()

3. Execution

Now let’s run the Python script:

ISAACSIM_PYTHON examples/1_px4_single_vehicle.py

This should open a stage with a blue ground-plane with an 3DR Iris vehicle model in it. The simulation should start playing automatically and the stage being rendered. PX4-Autopilot will start running automatically in the background, receiving data from the simulated sensors and sending target angular velocities for the vehicle rotors. You can now play with the vehicle using QGroundControl similarly to what was shown in Run in Extension Mode (GUI) tutorial.

To stop the simulation, you can either close the window, press the STOP button in the UI, or press Ctrl+C in the terminal.