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

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

@@ -28,7 +28,6 @@
     AccelStamped,
 )
 
-
 # set environment variable to use ROS2
 os.environ["RMW_IMPLEMENTATION"] = "rmw_cyclonedds_cpp"
 os.environ["ROS_DOMAIN_ID"] = "15"

exts/stride.simulator/stride/simulator/extension.py

@@ -9,7 +9,9 @@
 from omni import ui
 
 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, Go1
 from stride.simulator.params import SIMULATION_ENVIRONMENTS
 
 import asyncio
@@ -60,14 +62,22 @@ def on_environment():
                 def on_simulation():
                     async def respawn():
 
-                        self._anymal_config = AnymalCConfig()
+                        self._go1_config = Go1Config()
 
-                        self._anymal = AnymalC(
+                        self._anymal = Go1(
                             id=0,
                             init_pos=[0.0, 0.0, 0.7],
                             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/vehicles/quadrupedrobot/go1.py

@@ -0,0 +1,114 @@
+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.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
+
+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 = [
+            ROS2Backend(self.vehicle_name)
+        ]  # 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")