add Imu sensor to anymalc

exts/stride.simulator/stride/simulator/interfaces/__init__.py

@@ -0,0 +1 @@
+from .stride_sim_interface import StrideInterface

exts/stride.simulator/stride/simulator/vehicles/__init__.py

@@ -1 +1 @@
-"""PlACEHOLDER"""
+from .state import State

exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/anymalc.py

@@ -5,6 +5,8 @@
 
 # Get the location of the asset
 from stride.simulator.params import ROBOTS_ENVIRONMNETS
+from stride.simulator.vehicles.sensors import Imu
+from stride.simulator.interfaces import StrideInterface
 
 
 class AnymalCConfig(QuadrupedRobotConfig):
@@ -24,7 +26,7 @@ def __init__(self):
         self.usd_file = ROBOTS_ENVIRONMNETS["Anymal C"]
 
         # The default sensors for a quadrotor
-        self.sensors = []  # pylint: disable=use-list-literal FIXME
+        self.sensors = [Imu()]  # pylint: disable=use-list-literal FIXME
 
         # The backends for actually sending commands to the vehicle.
         # By default use mavlink (with default mavlink configurations)
@@ -34,6 +36,8 @@ def __init__(self):
 
 
 class AnymalC(QuadrupedRobot):
+    """AnymalC class - It is a child class of QuadrupedRobot class to implement a AnymalC robot in the simulator.
+    """
 
     def __init__(self, id: int, init_pos, init_orientation, config=AnymalCConfig()):
 
@@ -43,3 +47,31 @@ def __init__(self, id: int, init_pos, init_orientation, config=AnymalCConfig()):
             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._sensors = config.sensors
+        for sensor in self._sensors:
+            sensor.set_spherical_coordinate(StrideInterface().latitude, StrideInterface().longitude,
+                                            StrideInterface().altitude)
+
+        # Add callbacks to the physics engine to update each sensor at every timestep and let the sensor decide
+        # depending on its internal update rate whether to generate new data.
+        # TODO: Uncomment this when the physics engine is implemented.
+        # self._world.add_physics_callback(self._stage_prefix + "/Sensors", self.update_sensors)
+
+    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:
+            sensor_data = sensor.update(self._state, dt)
+
+            if sensor_data is not None:
+                print("TODO: Implement backend code.")

exts/stride.simulator/stride/simulator/vehicles/sensors/__init__.py

@@ -1 +1,2 @@
 from .sensor import Sensor
+from .imu import Imu

exts/stride.simulator/stride/simulator/vehicles/sensors/imu.py

@@ -1,19 +1,11 @@
-"""
-| File: imu.py
-| Author: Marcelo Jacinto ([email protected])
-| License: BSD-3-Clause. Copyright (c) 2023, Marcelo Jacinto. All rights reserved.
-| Description: Simulates an imu. Based on the implementation provided in PX4 stil_gazebo
-               (https://github.com/PX4/PX4-SITL_gazebo)
-"""
 __all__ = ["Imu"]
 
 import numpy as np
 from scipy.spatial.transform import Rotation
 
-from pegasus.simulator.logic.state import State
-from pegasus.simulator.logic.sensors import Sensor
-from pegasus.simulator.logic.rotations import rot_FLU_to_FRD, rot_ENU_to_NED
-from pegasus.simulator.logic.sensors.geo_mag_utils import GRAVITY_VECTOR
+from stride.simulator.vehicles.state import State
+from stride.simulator.vehicles.sensors import Sensor
+from stride.simulator.vehicles.sensors.geo_mag_utils import GRAVITY_VECTOR
 
 # TODO - test comment
 
@@ -41,13 +33,17 @@ def __init__(self, config=None):
             >>>         "bias_correlation_time": 300.0,
             >>>         "turn_on_bias_sigma": 20.0e-3 * 9.8
             >>> },
-            >>>  "update_rate": 1.0}                 # Hz
+            >>>  "update_frequency": 1.0}                 # Hz
         """
+        if config is None:
+            config = {}
+        else:
+            assert isinstance(config, dict), "The config parameter must be a dictionary."
 
-        # Initialize the Super class "object" attributes
-        super().__init__(sensor_type="Imu", update_rate=config.get("update_rate", 250.0))
+        # Initialize the Super class "object" attributes.
+        super().__init__(sensor_type="Imu", update_frequency=config.get("update_frequency", 250.0))
 
-        # Orientation noise constant
+        # Orientation noise constant.
         self._orientation_noise: float = 0.0
 
         # Gyroscope noise constants
@@ -58,15 +54,15 @@ def __init__(self, config=None):
         self._gyroscope_bias_correlation_time = gyroscope_config.get("bias_correlation_time", 1.0E3)
         self._gyroscope_turn_on_bias_sigma = gyroscope_config.get("turn_on_bias_sigma", 0.008726646259971648)
 
-        # Accelerometer noise constants
+        # Accelerometer noise constants.
         self._accelerometer_bias: np.ndarray = np.zeros((3,))
         accelerometer_config = config.get("accelerometer", {})
         self._accelerometer_noise_density = accelerometer_config.get("noise_density", 0.004)
         self._accelerometer_random_walk = accelerometer_config.get("random_walk", 0.006)
         self._accelerometer_bias_correlation_time = accelerometer_config.get("bias_correlation_time", 300.0)
         self._accelerometer_turn_on_bias_sigma = accelerometer_config.get("turn_on_bias_sigma", 0.196)
 
-        # Auxiliar variable used to compute the linear acceleration of the vehicle
+        # Auxiliar variable used to compute the linear acceleration of the vehicle.
         self._prev_linear_velocity = np.zeros((3,))
 
         # Save the current state measured by the Imu
@@ -83,15 +79,11 @@ def state(self):
         """
         return self._state
 
-    @Sensor.update_at_rate
+    @Sensor.update_at_frequency
     def update(self, state: State, dt: float):
         """Method that implements the logic of an Imu. In this method we start by generating the random walk of the
-        gyroscope. This value is then added to the real angular velocity of the vehicle (FLU relative to ENU inertial
-        frame expressed in FLU body frame). The same logic is followed for the accelerometer and the accelerations.
-        After this step, the angular velocity is rotated such that it expressed a FRD body frame, relative to a NED
-        inertial frame, expressed in the FRD body frame. Additionally, the acceleration is also rotated, such that it
-        becomes expressed in the body FRD frame of the vehicle. This sensor outputs data that follows the PX4 adopted
-        standard.
+        gyroscope. This value is then added to the real angular velocity of the vehicle (ENU inertial frame expressed in
+        FLU body frame). The same logic is followed for the accelerometer and the accelerations.
 
         Args:
             state (State): The current state of the vehicle.
@@ -101,10 +93,10 @@ def update(self, state: State, dt: float):
             (dict) A dictionary containing the current state of the sensor (the data produced by the sensor)
         """
 
-        # Gyroscopic terms
+        # Gyroscopic terms.
         tau_g: float = self._accelerometer_bias_correlation_time
 
-        # Discrete-time standard deviation equivalent to an "integrating" sampler with integration time dt
+        # Discrete-time standard deviation equivalent to an "integrating" sampler with integration time dt.
         sigma_g_d: float = 1 / np.sqrt(dt) * self._gyroscope_noise_density
         sigma_b_g: float = self._gyroscope_random_walk
 
@@ -139,14 +131,14 @@ def update(self, state: State, dt: float):
         linear_acceleration_inertial = (state.linear_velocity - self._prev_linear_velocity) / dt
         linear_acceleration_inertial = linear_acceleration_inertial - GRAVITY_VECTOR
 
-        # Update the previous linear velocity for the next computation
+        # Update the previous linear velocity for the next computation.
         self._prev_linear_velocity = state.linear_velocity
 
         # Compute the linear acceleration of the body frame, with respect to the inertial frame, expressed in the body
         # frame.
         linear_acceleration = np.array(Rotation.from_quat(state.attitude).inv().apply(linear_acceleration_inertial))
 
-        # Simulate the accelerometer noise processes and add them to the true linear aceleration values
+        # Simulate the accelerometer noise processes and add them to the true linear aceleration values.
         for i in range(3):
             self._accelerometer_bias[i] = phi_a_d * self._accelerometer_bias[i] + sigma_b_a_d * np.random.rand()
             linear_acceleration[i] = (
@@ -156,25 +148,16 @@ def update(self, state: State, dt: float):
         # TODO - Add small "noisy" to the attitude
 
         # --------------------------------------------------------------------------------------------
-        # Apply rotations such that we express the Imu data according to the FRD body frame convention
+        # Apply rotations such that we express the Imu data according to the FLU body frame convention
         # --------------------------------------------------------------------------------------------
 
-        # Convert the orientation to the FRD-NED standard
+        # Convert the orientation to the FLU-ENU standard
         attitude_flu_enu = Rotation.from_quat(state.attitude)
-        attitude_frd_enu = attitude_flu_enu * rot_FLU_to_FRD
-        attitude_frd_ned = rot_ENU_to_NED * attitude_frd_enu
-
-        # Convert the angular velocity from FLU to FRD standard
-        angular_velocity_frd = rot_FLU_to_FRD.apply(angular_velocity)
-
-        # Convert the linear acceleration in the body frame from FLU to FRD standard
-        linear_acceleration_frd = rot_FLU_to_FRD.apply(linear_acceleration)
 
-        # Add the values to the dictionary and return it
         self._state = {
-            "orientation": attitude_frd_ned.as_quat(),
-            "angular_velocity": angular_velocity_frd,
-            "linear_acceleration": linear_acceleration_frd,
+            "orientation": attitude_flu_enu.as_quat(),
+            "angular_velocity": angular_velocity,
+            "linear_acceleration": linear_acceleration,
         }
 
         return self._state

exts/stride.simulator/stride/simulator/vehicles/sensors/sensor.py

@@ -1,6 +1,6 @@
 __all__ = ["Sensor"]
 
-from stride.simulator.logic.state import State
+from stride.simulator.vehicles import State
 
 class Sensor:
     """The base class for implementing a sensor.