diff --git a/exts/stride.simulator/config/go1_cfg.yaml b/exts/stride.simulator/config/go1_cfg.yaml new file mode 100644 index 0000000..f949f2f --- /dev/null +++ b/exts/stride.simulator/config/go1_cfg.yaml @@ -0,0 +1,8 @@ + +sensor: + imu: + update_frequency: 250 + + lidar: + update_frequency: 25 + prim_path: /World/go1/lidar/lidar_PhysX \ No newline at end of file diff --git a/exts/stride.simulator/stride/simulator/backends/ros2_backend.py b/exts/stride.simulator/stride/simulator/backends/ros2_backend.py index ab55094..4567bb2 100644 --- a/exts/stride.simulator/stride/simulator/backends/ros2_backend.py +++ b/exts/stride.simulator/stride/simulator/backends/ros2_backend.py @@ -8,7 +8,7 @@ # # Perform some checks, because Isaac Sim some times does not play nice when using ROS/ROS2 disable_extension("omni.isaac.ros_bridge") -enable_extension("omni.isaac.ros2_bridge-humble") +enable_extension("omni.isaac.ros2_bridge") # Inform the user that now we are actually import the ROS2 dependencies # Note: we are performing the imports here to make sure that ROS2 extension was load correctly @@ -28,7 +28,6 @@ AccelStamped, ) - # set environment variable to use ROS2 os.environ["RMW_IMPLEMENTATION"] = "rmw_cyclonedds_cpp" os.environ["ROS_DOMAIN_ID"] = "15" @@ -254,20 +253,3 @@ def update_state(self, state): self.twist_pub.publish(twist) self.twist_inertial_pub.publish(twist_inertial) self.accel_pub.publish(accel) - - # def check_ros_extension(self): - # """ - # Method that checks which ROS extension is installed. - # """ - - # # Get the handle for the extension manager - # extension_manager = omni.kit.app.get_app().get_extension_manager() - - # version = "" - - # if self._ext_manager.is_extension_enabled("omni.isaac.ros_bridge"): - # version = "ros" - # elif self._ext_manager.is_extension_enabled("omni.isaac.ros2_bridge"): - # version = "ros2" - # else: - # carb.log_warn("Neither extension 'omni.isaac.ros_bridge' nor 'omni.isaac.ros2_bridge' is enabled") diff --git a/exts/stride.simulator/stride/simulator/extension.py b/exts/stride.simulator/stride/simulator/extension.py index 6cda288..ebf1ce6 100644 --- a/exts/stride.simulator/stride/simulator/extension.py +++ b/exts/stride.simulator/stride/simulator/extension.py @@ -8,8 +8,16 @@ import omni.kit.app from omni import ui +from stride.simulator.backends import ROS2Backend + from stride.simulator.interfaces.stride_sim_interface import StrideInterface -from stride.simulator.vehicles.quadrupedrobot.anymalc import AnymalC, AnymalCConfig + +# from stride.simulator.vehicles.quadrupedrobot.anymalc import AnymalC, AnymalCConfig + +from stride.simulator.vehicles.quadrupedrobot.go1 import Go1Config + +from stride.simulator.vehicles.quadrupedrobot.go1 import Go1 + from stride.simulator.params import SIMULATION_ENVIRONMENTS import asyncio @@ -60,14 +68,26 @@ def on_environment(): def on_simulation(): async def respawn(): - self._anymal_config = AnymalCConfig() + self._go1_config = Go1Config() + + self._go1_config.backends = [ + ROS2Backend(self._go1_config.vehicle_name) + ] - self._anymal = AnymalC( + self._go1 = Go1( id=0, - init_pos=[0.0, 0.0, 0.7], + init_pos=[0.0, 0.0, 0.5], init_orientation=[0.0, 0.0, 0.0, 1.0], - config=self._anymal_config, + config=self._go1_config, ) + # self._anymal_config = AnymalCConfig() + + # self._anymal = AnymalC( + # id=0, + # init_pos=[0.0, 0.0, 0.7], + # init_orientation=[0.0, 0.0, 0.0, 1.0], + # config=self._anymal_config, + # ) self._current_tasks = self._stride_sim.world.get_current_tasks() await self._stride_sim.world.reset_async() diff --git a/exts/stride.simulator/stride/simulator/params.py b/exts/stride.simulator/stride/simulator/params.py index 6d4f898..c0051ef 100644 --- a/exts/stride.simulator/stride/simulator/params.py +++ b/exts/stride.simulator/stride/simulator/params.py @@ -57,7 +57,7 @@ # Define the default settings for the simulation environment DEFAULT_WORLD_SETTINGS = { - "physics_dt": 1.0 / 200.0, + "physics_dt": 1.0 / 800.0, "stage_units_in_meters": 1.0, "rendering_dt": 1.0 / 60.0, } diff --git a/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/__init__.py b/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/__init__.py index bdd1b4c..8245cfd 100644 --- a/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/__init__.py +++ b/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/__init__.py @@ -1 +1,3 @@ +from .quadrupedrobot import QuadrupedRobot, QuadrupedRobotConfig from .anymalc import AnymalC, AnymalCConfig +from .go1 import Go1, Go1Config diff --git a/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/anymalc.py b/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/anymalc.py index 798f8f2..dcdb2dd 100644 --- a/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/anymalc.py +++ b/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/anymalc.py @@ -8,7 +8,6 @@ # Get the location of the asset # from stride.simulator.backends import LoggerBackend -from stride.simulator.backends import ROS2Backend from stride.simulator.params import ROBOTS from stride.simulator.vehicles.sensors.imu import Imu from stride.simulator.vehicles.sensors.lidar import Lidar @@ -55,9 +54,7 @@ def __init__(self): # The backends for actually sending commands to the vehicle. # It can also be a ROS2 backend or your own custom Backend implementation! - self.backends = [ - ROS2Backend(self.vehicle_name) - ] # pylint: disable=use-list-literal + self.backends = [] # pylint: disable=use-list-literal class AnymalC(QuadrupedRobot): diff --git a/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/go1.py b/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/go1.py new file mode 100644 index 0000000..382a819 --- /dev/null +++ b/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/go1.py @@ -0,0 +1,111 @@ +from stride.simulator.vehicles.quadrupedrobot.quadrupedrobot import ( + QuadrupedRobot, + QuadrupedRobotConfig, +) + +# Mavlink interface +# from stride.simulator.logic.backends.mavlink_backend import MavlinkBackend + +# Get the location of the asset +# from stride.simulator.backends import LoggerBackend +from stride.simulator.params import ROBOTS +from stride.simulator.vehicles.sensors.imu import Imu + +# from stride.simulator.vehicles.sensors.lidar import Lidar + +from stride.simulator.vehicles.controllers.anymal_controller import AnyamlController + +import yaml +import os + + +class Go1Config(QuadrupedRobotConfig): + """ + Go1 configuration class + """ + + def __init__(self): + + super().__init__() + + self.vehicle_name = "go1" + + # Get the path to the "" directory + stridesim_dir = os.path.abspath(__file__) + for _ in range(5): + stridesim_dir = os.path.dirname(stridesim_dir) + + # Stage prefix of the vehicle when spawning in the world + self.stage_prefix = "/World/Go1" + + # The USD file that describes the visual aspect of the vehicle + self.usd_file = ROBOTS["go1"] + + # read config file + with open( + stridesim_dir + "/config/go1_cfg.yaml", "r", encoding="utf-8" + ) as file: + self.config = yaml.safe_load(file) + + # The default sensors for a Go1 + self.sensors = [ + Imu(self.config["sensor"]["imu"]), + # Lidar(self.config["sensor"]["lidar"]), + ] # pylint: disable=use-list-literal FIXME + + # The backends for actually sending commands to the vehicle. + # It can also be a ROS2 backend or your own custom Backend implementation! + self.backends = [] # pylint: disable=use-list-literal + + +class Go1(QuadrupedRobot): + """Go1 class - It is a child class of QuadrupedRobot class to implement a Go1 robot in the simulator.""" + + def __init__(self, id: int, init_pos, init_orientation, config=Go1Config()): + + if init_pos is None: + init_pos = [0.0, 0.0, 0.5] + if init_orientation is None: + init_orientation = [0.0, 0.0, 0.0, 1.0] + + super().__init__( + config.stage_prefix, + config.usd_file, + id, + init_pos, + init_orientation, + config=config, + ) + + self.controller = AnyamlController() + + def update_sensors(self, dt: float): + """Callback that is called at every physics steps and will call the sensor.update method to generate new + sensor data. For each data that the sensor generates, the backend.update_sensor method will also be called for + every backend. For example, if new data is generated for an IMU and we have a MavlinkBackend, then the + update_sensor method will be called for that backend so that this data can latter be sent thorugh mavlink. + + Args: + dt (float): The time elapsed between the previous and current function calls (s). + """ + + # Call the update method for the sensor to update its values internally (if applicable) + for sensor in self._sensors: + try: + sensor_data = sensor.update(self._state, dt) + except Exception as e: # pylint: disable=broad-except + print(f"Error updating sensor: {e}") + continue + + if sensor_data is not None: + for backend in self._backends: + backend.update_sensor(sensor.sensor_type, sensor_data) + + def initialize(self, physics_sim_view=None) -> None: + """[summary] + + initialize the dc interface, set up drive mode + """ + super().initialize(physics_sim_view=physics_sim_view) + self.get_articulation_controller().set_effort_modes("force") + self.get_articulation_controller().switch_control_mode("effort") diff --git a/exts/stride.simulator/stride/simulator/vehicles/sensors/lidar.py b/exts/stride.simulator/stride/simulator/vehicles/sensors/lidar.py index f4b8ab6..be4fab5 100644 --- a/exts/stride.simulator/stride/simulator/vehicles/sensors/lidar.py +++ b/exts/stride.simulator/stride/simulator/vehicles/sensors/lidar.py @@ -78,7 +78,7 @@ def update(self, state: State, dt: float): self._lidar.set_resolution([0.4, 0.4]) self._lidar.set_valid_range([0.1, 6]) self._lidar.enable_visualization( - high_lod=True, draw_points=True, draw_lines=False + high_lod=True, draw_points=False, draw_lines=False ) self.lidar_flag_ = True