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 4334ef3..4567bb2 100644 --- a/exts/stride.simulator/stride/simulator/backends/ros2_backend.py +++ b/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" diff --git a/exts/stride.simulator/stride/simulator/extension.py b/exts/stride.simulator/stride/simulator/extension.py index 6cda288..fbf3523 100644 --- a/exts/stride.simulator/stride/simulator/extension.py +++ b/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() 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..f835a20 --- /dev/null +++ b/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")