From 94851aff3be0da2921c75f1a6aa892d552b91905 Mon Sep 17 00:00:00 2001 From: jinwon kim Date: Thu, 1 Feb 2024 21:41:04 +0900 Subject: [PATCH 1/2] Add physics spawning code --- .../stride/simulator/extension.py | 22 ++++++++++++++----- .../vehicles/quadrupedrobot/quadrupedrobot.py | 3 +++ .../stride/simulator/vehicles/vehicle.py | 7 +++--- 3 files changed, 24 insertions(+), 8 deletions(-) diff --git a/exts/stride.simulator/stride/simulator/extension.py b/exts/stride.simulator/stride/simulator/extension.py index c94b8eb..e803dc9 100644 --- a/exts/stride.simulator/stride/simulator/extension.py +++ b/exts/stride.simulator/stride/simulator/extension.py @@ -18,6 +18,7 @@ from stride.simulator.params import SIMULATION_ENVIRONMENTS import asyncio +import carb # Functions and vars are available to other extension as usual in python: `example.python_ext.some_public_function(x)` def some_public_function(x: int): @@ -59,18 +60,29 @@ def on_click(): self.autoload_helper() label.text = "Initialize world" - + asyncio.ensure_future(self._stride_sim.load_environment_async( SIMULATION_ENVIRONMENTS["Default Environment"], force_clear=True)) def on_spawn(): + + async def async_load_vehicle(): + + # Check if we already have a physics environment activated. If not, then activate it + # and only after spawn the vehicle. This is to avoid trying to spawn a vehicle without a physics + # environment setup. This way we can even spawn a vehicle in an empty world and it won't care + if hasattr(self._stride_sim.world, "_physics_context") == False: + await self._stride_sim.world.initialize_simulation_context_async() + + self._anymal_config = AnymalCConfig() - self._anymal_config = AnymalCConfig() + self._anymal = AnymalC(id=ext_id, init_pos=[0.0, 0.0, 0.5],init_orientation=[0.0, 0.0, 0.0, 1.0], + config=self._anymal_config) - self._anymal = AnymalC(id=ext_id, init_pos=[0.0, 0.0, 0.5],init_orientation=[0.0, 0.0, 0.0, 1.0], - config=self._anymal_config) + # Run the actual vehicle spawn async so that the UI does not freeze + asyncio.ensure_future(async_load_vehicle()) - label.text = "Open environment" + label.text = "Load vehicle" with ui.HStack(): ui.Button("Init", clicked_fn=on_click) diff --git a/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/quadrupedrobot.py b/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/quadrupedrobot.py index af4a085..174927a 100644 --- a/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/quadrupedrobot.py +++ b/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/quadrupedrobot.py @@ -140,6 +140,9 @@ def update(self, dt: float): Args: dt (float): The time elapsed between the previous and current function calls (s). """ + + import ipdb # pylint: disable=import-outside-toplevel + ipdb.set_trace() # Get the desired base velocity for robot from the first backend (can be mavlink or other) expressed in m/s if len(self._backends) != 0: diff --git a/exts/stride.simulator/stride/simulator/vehicles/vehicle.py b/exts/stride.simulator/stride/simulator/vehicles/vehicle.py index f27abfa..60849cf 100644 --- a/exts/stride.simulator/stride/simulator/vehicles/vehicle.py +++ b/exts/stride.simulator/stride/simulator/vehicles/vehicle.py @@ -101,18 +101,19 @@ def __init__( # pylint: disable=dangerous-default-value FIXME self._state = State() # Add a callback to the physics engine to update the current state of the system - # self._world.add_physics_callback(self._stage_prefix + "/state", self.update_state) + self._world.add_physics_callback(self._stage_prefix + "/state", self.update_state) # Add the update method to the physics callback if the world was received # so that we can apply forces and torques to the vehicle. Note, this method should # be implemented in classes that inherit the vehicle object - # self._world.add_physics_callback(self._stage_prefix + "/update", self.update) + self._world.add_physics_callback(self._stage_prefix + "/update", self.update) # Set the flag that signals if the simulation is running or not self._sim_running = False # Add a callback to start/stop of the simulation once the play/stop button is hit - # self._world.add_timeline_callback(self._stage_prefix + "/start_stop_sim", self.sim_start_stop) + self._world.add_timeline_callback(self._stage_prefix + "/start_sim", self.sim_start) + self._world.add_timeline_callback(self._stage_prefix + "/stop_sim", self.sim_stop) def __del__(self): """ From c043aeb0d42853facdaaddc9772770d92901c20d Mon Sep 17 00:00:00 2001 From: jinwon kim Date: Mon, 5 Feb 2024 04:29:32 +0900 Subject: [PATCH 2/2] debug reset function and docking window tab to property --- exts/stride.simulator/stride/simulator/extension.py | 4 +++- .../simulator/vehicles/quadrupedrobot/quadrupedrobot.py | 7 +++++++ 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/exts/stride.simulator/stride/simulator/extension.py b/exts/stride.simulator/stride/simulator/extension.py index 8e2155d..0758d4f 100644 --- a/exts/stride.simulator/stride/simulator/extension.py +++ b/exts/stride.simulator/stride/simulator/extension.py @@ -33,6 +33,8 @@ def on_startup(self, ext_id): self._window = ui.Window("Stride Simulator", width=300, height=300) + self._window.deferred_dock_in("Property", ui.DockPolicy.CURRENT_WINDOW_IS_ACTIVE) + # Start the extension backend self._stride_sim = StrideInterface() @@ -58,7 +60,7 @@ async def respawn(): self._anymal_config = AnymalCConfig() self._anymal = AnymalC(id=0, - init_pos=[0.0, 0.0, 0.8], + init_pos=[0.0, 0.0, 0.7], init_orientation=[0.0, 0.0, 0.0, 1.0], config=self._anymal_config) diff --git a/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/quadrupedrobot.py b/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/quadrupedrobot.py index 7ad4f83..9eddf1c 100644 --- a/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/quadrupedrobot.py +++ b/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/quadrupedrobot.py @@ -6,6 +6,7 @@ from omni.isaac.core.utils.rotations import quat_to_rot_matrix, quat_to_euler_angles, euler_to_rot_matrix from pxr import Gf import numpy as np +import asyncio class QuadrupedRobotConfig: @@ -142,6 +143,12 @@ def stop(self): for backend in self._backends: backend.stop() + async def reset_async(): + await self._world.reset_async() + await self._world.pause_async() + + asyncio.ensure_future(reset_async()) + def update(self, dt: float): """ Method that computes and applies the torques to the vehicle in simulation based on the base body speed.