Feat/2023.1.1 (#46)

* feat: adapt code to version 2023.1.1 (#42)

* Feature/go1 (#44)

* feat: adapt code to version 2023.1.1

* feat: add go1
However, It doesn't have any controller yet.
You can add walk these ways controller here

* Bug/ros2 node duplicate (#45)

* feat: adapt code to version 2023.1.1

* feat: add go1
However, It doesn't have any controller yet.
You can add walk these ways controller here

* bug: fix duplicated ros node problem

* fix: pylint error(cyclic import)

* fix2: pylint error(cyclic import)

* fix3: pylint error(cyclic import)

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

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")

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()

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,
 }

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

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):

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")

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