diff --git a/exts/stride.simulator/stride/simulator/backends/backend.py b/exts/stride.simulator/stride/simulator/backends/backend.py index 6349b9c..cfd47df 100644 --- a/exts/stride.simulator/stride/simulator/backends/backend.py +++ b/exts/stride.simulator/stride/simulator/backends/backend.py @@ -11,8 +11,7 @@ class Backend: """ def __init__(self): - """Initialize the Backend class - """ + """Initialize the Backend class""" self._vehicle = None @property @@ -70,16 +69,13 @@ def update(self, dt: float): pass def start(self): - """Method that when implemented should handle the begining of the simulation of vehicle - """ + """Method that when implemented should handle the begining of the simulation of vehicle""" pass def stop(self): - """Method that when implemented should handle the stopping of the simulation of vehicle - """ + """Method that when implemented should handle the stopping of the simulation of vehicle""" pass def reset(self): - """Method that when implemented, should handle the reset of the vehicle simulation to its original state - """ + """Method that when implemented, should handle the reset of the vehicle simulation to its original state""" pass diff --git a/exts/stride.simulator/stride/simulator/backends/logger_backend.py b/exts/stride.simulator/stride/simulator/backends/logger_backend.py index bb29b9f..98ce5a2 100644 --- a/exts/stride.simulator/stride/simulator/backends/logger_backend.py +++ b/exts/stride.simulator/stride/simulator/backends/logger_backend.py @@ -28,7 +28,9 @@ def __init__(self, config=None): if config is None: config = {} else: - assert isinstance(config, dict), "The config parameter must be a dictionary." + assert isinstance( + config, dict + ), "The config parameter must be a dictionary." self.vehicle_id = config.get("vehicle_id", 0) self.update_rate: float = config.get("update_rate", 250.0) # [Hz] @@ -73,7 +75,9 @@ def update_sensor(self, sensor_type: str, data): elif sensor_type == "Lidar": self.update_lidar_data(data) else: - carb.log_warn(f"Sensor type {sensor_type} is not supported by the ROS2 backend.") + carb.log_warn( + f"Sensor type {sensor_type} is not supported by the ROS2 backend." + ) pass # TODO: Add support for other sensors diff --git a/exts/stride.simulator/stride/simulator/backends/ros2_backend.py b/exts/stride.simulator/stride/simulator/backends/ros2_backend.py index 0078b8e..ab55094 100644 --- a/exts/stride.simulator/stride/simulator/backends/ros2_backend.py +++ b/exts/stride.simulator/stride/simulator/backends/ros2_backend.py @@ -1,4 +1,3 @@ - import os import carb import numpy as np @@ -16,15 +15,25 @@ import rclpy # pylint: disable=wrong-import-position from std_msgs.msg import Float64 # pylint: disable=unused-import, wrong-import-position from sensor_msgs.msg import ( # pylint: disable=unused-import, wrong-import-position - Imu, PointCloud2, PointField, MagneticField, NavSatFix, NavSatStatus + Imu, + PointCloud2, + PointField, + MagneticField, + NavSatFix, + NavSatStatus, +) +from geometry_msgs.msg import ( # pylint: disable=wrong-import-position + PoseStamped, + TwistStamped, + AccelStamped, ) -from geometry_msgs.msg import PoseStamped, TwistStamped, AccelStamped # pylint: disable=wrong-import-position # set environment variable to use ROS2 os.environ["RMW_IMPLEMENTATION"] = "rmw_cyclonedds_cpp" os.environ["ROS_DOMAIN_ID"] = "15" + class ROS2Backend(Backend): """ A class representing the ROS2 backend for the simulation. @@ -62,17 +71,25 @@ def __init__(self, node_name: str): rclpy.init() self.node = rclpy.create_node(node_name) - - # Create publishers for the state of the vehicle in ENU - self.pose_pub = self.node.create_publisher(PoseStamped, node_name + "/state/pose", 10) - self.twist_pub = self.node.create_publisher(TwistStamped, node_name + "/state/twist", 10) - self.twist_inertial_pub = self.node.create_publisher(TwistStamped, node_name + "/state/twist_inertial", 10) - self.accel_pub = self.node.create_publisher(AccelStamped, node_name + "/state/accel", 10) + self.pose_pub = self.node.create_publisher( + PoseStamped, node_name + "/state/pose", 10 + ) + self.twist_pub = self.node.create_publisher( + TwistStamped, node_name + "/state/twist", 10 + ) + self.twist_inertial_pub = self.node.create_publisher( + TwistStamped, node_name + "/state/twist_inertial", 10 + ) + self.accel_pub = self.node.create_publisher( + AccelStamped, node_name + "/state/accel", 10 + ) # Create publishers for some sensor data self.imu_pub = self.node.create_publisher(Imu, node_name + "/sensors/imu", 10) - self.point_cloud_pub = self.node.create_publisher(PointCloud2, node_name + "/sensors/points", 10) + self.point_cloud_pub = self.node.create_publisher( + PointCloud2, node_name + "/sensors/points", 10 + ) def update(self, dt: float): """ @@ -127,7 +144,9 @@ def update_lidar_data(self, data): msg = PointCloud2() # Flatten LiDAR data - points_flat = np.array(data["points"]).reshape(-1, 3) # Adjust based on your data's structure + points_flat = np.array(data["points"]).reshape( + -1, 3 + ) # Adjust based on your data's structure # Create a PointCloud2 message msg = PointCloud2() @@ -139,7 +158,7 @@ def update_lidar_data(self, data): msg.fields = [ PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1), PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1), - PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1) + PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1), ] msg.is_bigendian = False msg.point_step = 12 # Float32, x, y, z @@ -170,7 +189,9 @@ def update_sensor(self, sensor_type: str, data): elif sensor_type == "Lidar": self.update_lidar_data(data) else: - carb.log_warn(f"Sensor type {sensor_type} is not supported by the ROS2 backend.") + carb.log_warn( + f"Sensor type {sensor_type} is not supported by the ROS2 backend." + ) pass def update_state(self, state): @@ -187,7 +208,9 @@ def update_state(self, state): accel = AccelStamped() # Update the header - pose.header.stamp = (self.node.get_clock().now().to_msg()) # TODO: fill time when the state was measured. + pose.header.stamp = ( + self.node.get_clock().now().to_msg() + ) # TODO: fill time when the state was measured. twist.header.stamp = pose.header.stamp twist_inertial.header.stamp = pose.header.stamp accel.header.stamp = pose.header.stamp diff --git a/exts/stride.simulator/stride/simulator/extension.py b/exts/stride.simulator/stride/simulator/extension.py index 0758d4f..6cda288 100644 --- a/exts/stride.simulator/stride/simulator/extension.py +++ b/exts/stride.simulator/stride/simulator/extension.py @@ -33,7 +33,9 @@ def on_startup(self, ext_id): self._window = ui.Window("Stride Simulator", width=300, height=300) - self._window.deferred_dock_in("Property", ui.DockPolicy.CURRENT_WINDOW_IS_ACTIVE) + self._window.deferred_dock_in( + "Property", ui.DockPolicy.CURRENT_WINDOW_IS_ACTIVE + ) # Start the extension backend self._stride_sim = StrideInterface() @@ -49,27 +51,32 @@ def on_world(): def on_environment(): - self._stride_sim.load_asset(SIMULATION_ENVIRONMENTS["Default Environment"], "/World/layout") + self._stride_sim.load_asset( + SIMULATION_ENVIRONMENTS["Default Environment"], "/World/layout" + ) label.text = "Load environment" def on_simulation(): - async def respawn(): 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._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() await self._stride_sim.world.pause_async() if len(self._current_tasks) > 0: - self._stride_sim.world.add_physics_callback("tasks_step", self._world.step_async) + self._stride_sim.world.add_physics_callback( + "tasks_step", self._world.step_async + ) asyncio.ensure_future(respawn()) diff --git a/exts/stride.simulator/stride/simulator/interfaces/stride_sim_interface.py b/exts/stride.simulator/stride/simulator/interfaces/stride_sim_interface.py index 93aeec5..386ee43 100644 --- a/exts/stride.simulator/stride/simulator/interfaces/stride_sim_interface.py +++ b/exts/stride.simulator/stride/simulator/interfaces/stride_sim_interface.py @@ -103,8 +103,7 @@ def altitude(self): return self._altitude def initialize_world(self): - """Method that initializes the world object - """ + """Method that initializes the world object""" async def _on_load_world_async(): if self._world is None: @@ -243,7 +242,12 @@ def load_asset(self, usd_asset: str, stage_prefix: str): success = prim.GetReferences().AddReference(usd_asset) if not success: - raise Exception("The usd asset" + usd_asset + "is not load at stage path " + stage_prefix) + raise Exception( + "The usd asset" + + usd_asset + + "is not load at stage path " + + stage_prefix + ) def set_viewport_camera(self, camera_position, camera_target): """Sets the viewport camera to given position and makes it point to another target position. @@ -256,7 +260,9 @@ def set_viewport_camera(self, camera_position, camera_target): # Set the camera view to a fixed value set_camera_view(eye=camera_position, target=camera_target) - def set_world_settings(self, physics_dt=None, stage_units_in_meters=None, rendering_dt=None): + def set_world_settings( + self, physics_dt=None, stage_units_in_meters=None, rendering_dt=None + ): """ Set the current world settings to the pre-defined settings. TODO - finish the implementation of this method. For now these new setting will never override the default ones. diff --git a/exts/stride.simulator/stride/simulator/params.py b/exts/stride.simulator/stride/simulator/params.py index 51148fa..6d4f898 100644 --- a/exts/stride.simulator/stride/simulator/params.py +++ b/exts/stride.simulator/stride/simulator/params.py @@ -51,7 +51,13 @@ # Add the Isaac Sim assets to the list for asset, path in NVIDIA_SIMULATION_ENVIRONMENTS.items(): - SIMULATION_ENVIRONMENTS[asset] = NVIDIA_ASSETS_PATH + ISAAC_SIM_ENVIRONMENTS + "/" + path + SIMULATION_ENVIRONMENTS[asset] = ( + NVIDIA_ASSETS_PATH + ISAAC_SIM_ENVIRONMENTS + "/" + path + ) # Define the default settings for the simulation environment -DEFAULT_WORLD_SETTINGS = {"physics_dt": 1.0 / 200.0, "stage_units_in_meters": 1.0, "rendering_dt": 1.0 / 60.0} +DEFAULT_WORLD_SETTINGS = { + "physics_dt": 1.0 / 200.0, + "stage_units_in_meters": 1.0, + "rendering_dt": 1.0 / 60.0, +} diff --git a/exts/stride.simulator/stride/simulator/tests/test_hello_world.py b/exts/stride.simulator/stride/simulator/tests/test_hello_world.py index 91e48af..bd83d00 100644 --- a/exts/stride.simulator/stride/simulator/tests/test_hello_world.py +++ b/exts/stride.simulator/stride/simulator/tests/test_hello_world.py @@ -26,7 +26,6 @@ async def test_hello_public_function(self): result = stride.simulator.some_public_function(4) self.assertEqual(result, 256) - async def test_window_button(self): # Find a label in our window @@ -34,7 +33,9 @@ async def test_window_button(self): # Find buttons in our window add_button = ui_test.find("Stride Simulator//Frame/**/Button[*].text=='Add'") - reset_button = ui_test.find("Stride Simulator//Frame/**/Button[*].text=='Reset'") + reset_button = ui_test.find( + "Stride Simulator//Frame/**/Button[*].text=='Reset'" + ) # Click reset button await reset_button.click() diff --git a/exts/stride.simulator/stride/simulator/vehicles/controllers/anymal_controller.py b/exts/stride.simulator/stride/simulator/vehicles/controllers/anymal_controller.py index 3b4b6c0..a5fbd78 100644 --- a/exts/stride.simulator/stride/simulator/vehicles/controllers/anymal_controller.py +++ b/exts/stride.simulator/stride/simulator/vehicles/controllers/anymal_controller.py @@ -2,7 +2,9 @@ from omni.isaac.core.utils.nucleus import get_assets_root_path from stride.simulator.vehicles.controllers.controller import Controller -from stride.simulator.vehicles.controllers.networks.actuator_network import LstmSeaNetwork +from stride.simulator.vehicles.controllers.networks.actuator_network import ( + LstmSeaNetwork, +) import io import numpy as np @@ -25,8 +27,9 @@ def __init__(self): assets_root_path = get_assets_root_path() # Policy - file_content = omni.client.read_file(assets_root_path + - "/Isaac/Samples/Quadruped/Anymal_Policies/policy_1.pt")[2] + file_content = omni.client.read_file( + assets_root_path + "/Isaac/Samples/Quadruped/Anymal_Policies/policy_1.pt" + )[2] file = io.BytesIO(memoryview(file_content).tobytes()) self._policy = torch.jit.load(file) @@ -36,13 +39,17 @@ def __init__(self): self.base_vel_ang_scale = 0.25 self.joint_pos_scale = 1.0 self.joint_vel_scale = 0.05 - self.default_joint_pos = np.array([0.0, 0.4, -0.8, 0.0, -0.4, 0.8, -0.0, 0.4, -0.8, -0.0, -0.4, 0.8]) + self.default_joint_pos = np.array( + [0.0, 0.4, -0.8, 0.0, -0.4, 0.8, -0.0, 0.4, -0.8, -0.0, -0.4, 0.8] + ) self.previous_action = np.zeros(12) self._policy_counter = 0 # Actuator network - file_content = omni.client.read_file(assets_root_path + - "/Isaac/Samples/Quadruped/Anymal_Policies/sea_net_jit2.pt")[2] + file_content = omni.client.read_file( + assets_root_path + + "/Isaac/Samples/Quadruped/Anymal_Policies/sea_net_jit2.pt" + )[2] file = io.BytesIO(memoryview(file_content).tobytes()) self._actuator_network = LstmSeaNetwork() self._actuator_network.setup(file, self.default_joint_pos) @@ -90,8 +97,9 @@ def advance(self, dt, obs, command): current_joint_vel = self.state.joint_velocities current_joint_pos = np.array(current_joint_pos.reshape([3, 4]).T.flat) current_joint_vel = np.array(current_joint_vel.reshape([3, 4]).T.flat) - joint_torques, _ = self._actuator_network.compute_torques(current_joint_pos, current_joint_vel, - self._action_scale * self.action) + joint_torques, _ = self._actuator_network.compute_torques( + current_joint_pos, current_joint_vel, self._action_scale * self.action + ) self._policy_counter += 1 diff --git a/exts/stride.simulator/stride/simulator/vehicles/controllers/networks/actuator_network.py b/exts/stride.simulator/stride/simulator/vehicles/controllers/networks/actuator_network.py index dcdf20b..99f71a2 100644 --- a/exts/stride.simulator/stride/simulator/vehicles/controllers/networks/actuator_network.py +++ b/exts/stride.simulator/stride/simulator/vehicles/controllers/networks/actuator_network.py @@ -35,11 +35,15 @@ def reset(self): self._cell_state[:, :, :] = 0.0 @torch.no_grad() - def compute_torques(self, joint_pos, joint_vel, actions) -> Tuple[np.ndarray, np.ndarray]: + def compute_torques( + self, joint_pos, joint_vel, actions + ) -> Tuple[np.ndarray, np.ndarray]: # create sea network input obs actions = actions.copy() actuator_net_input = torch.zeros((12, 1, 2)) - actuator_net_input[:, 0, 0] = torch.from_numpy(actions + self._default_joint_pos - joint_pos) + actuator_net_input[:, 0, 0] = torch.from_numpy( + actions + self._default_joint_pos - joint_pos + ) actuator_net_input[:, 0, 1] = torch.from_numpy(np.clip(joint_vel, -20.0, 20)) # call the network torques, (self._hidden_state, self._cell_state) = self._network( @@ -106,11 +110,15 @@ def _load_weights(self, weights_path: str): data[idx : idx + layer.in_features * layer.out_features], newshape=(layer.in_features, layer.out_features), ).T - layer.weight = torch.nn.Parameter(torch.from_numpy(weight.astype(np.float32))) + layer.weight = torch.nn.Parameter( + torch.from_numpy(weight.astype(np.float32)) + ) idx += layer.out_features * layer.in_features # layer biases bias = data[idx : idx + layer.out_features] - layer.bias = torch.nn.Parameter(torch.from_numpy(bias.astype(np.float32))) + layer.bias = torch.nn.Parameter( + torch.from_numpy(bias.astype(np.float32)) + ) idx += layer.out_features # set the module in eval mode self.eval() @@ -120,22 +128,44 @@ def _update_joint_history(self, joint_pos, joint_vel, actions): joint_pos = np.asarray(joint_pos) joint_vel = np.asarray(joint_vel) # compute error in position - joint_pos_error = self._action_scale * actions + self._default_joint_pos - joint_pos + joint_pos_error = ( + self._action_scale * actions + self._default_joint_pos - joint_pos + ) # store into history - self._joint_pos_history[:, : self._history_size] = self._joint_pos_history[:, 1:] - self._joint_vel_history[:, : self._history_size] = self._joint_vel_history[:, 1:] + self._joint_pos_history[:, : self._history_size] = self._joint_pos_history[ + :, 1: + ] + self._joint_vel_history[:, : self._history_size] = self._joint_vel_history[ + :, 1: + ] self._joint_pos_history[:, self._history_size] = joint_pos_error self._joint_vel_history[:, self._history_size] = joint_vel def _compute_sea_torque(self): inp = torch.zeros((12, 6)) for dof in range(12): - inp[dof, 0] = self._sea_vel_scale * self._joint_vel_history[dof, self._history_size - self._delays[0]] - inp[dof, 1] = self._sea_vel_scale * self._joint_vel_history[dof, self._history_size - self._delays[1]] - inp[dof, 2] = self._sea_vel_scale * self._joint_vel_history[dof, self._history_size] - inp[dof, 3] = self._sea_pos_scale * self._joint_pos_history[dof, self._history_size - self._delays[0]] - inp[dof, 4] = self._sea_pos_scale * self._joint_pos_history[dof, self._history_size - self._delays[1]] - inp[dof, 5] = self._sea_pos_scale * self._joint_pos_history[dof, self._history_size] + inp[dof, 0] = ( + self._sea_vel_scale + * self._joint_vel_history[dof, self._history_size - self._delays[0]] + ) + inp[dof, 1] = ( + self._sea_vel_scale + * self._joint_vel_history[dof, self._history_size - self._delays[1]] + ) + inp[dof, 2] = ( + self._sea_vel_scale * self._joint_vel_history[dof, self._history_size] + ) + inp[dof, 3] = ( + self._sea_pos_scale + * self._joint_pos_history[dof, self._history_size - self._delays[0]] + ) + inp[dof, 4] = ( + self._sea_pos_scale + * self._joint_pos_history[dof, self._history_size - self._delays[1]] + ) + inp[dof, 5] = ( + self._sea_pos_scale * self._joint_pos_history[dof, self._history_size] + ) return self._sea_output_scale * self._sea_network(inp) diff --git a/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/anymalc.py b/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/anymalc.py index d6c69c3..798f8f2 100644 --- a/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/anymalc.py +++ b/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/anymalc.py @@ -1,4 +1,7 @@ -from stride.simulator.vehicles.quadrupedrobot.quadrupedrobot import QuadrupedRobot, QuadrupedRobotConfig +from stride.simulator.vehicles.quadrupedrobot.quadrupedrobot import ( + QuadrupedRobot, + QuadrupedRobotConfig, +) # Mavlink interface # from stride.simulator.logic.backends.mavlink_backend import MavlinkBackend @@ -15,6 +18,7 @@ import yaml import os + class AnymalCConfig(QuadrupedRobotConfig): """ AnymalC configuration class @@ -38,21 +42,26 @@ def __init__(self): self.usd_file = ROBOTS["Anymal C"] # read config file - with open(stridesim_dir + "/config/anymalc_cfg.yaml", "r", encoding="utf-8") as file: + with open( + stridesim_dir + "/config/anymalc_cfg.yaml", "r", encoding="utf-8" + ) as file: self.config = yaml.safe_load(file) # The default sensors for a Anymal C - self.sensors = [Imu(self.config["sensor"]["imu"]), Lidar(self.config["sensor"]["lidar"])] # pylint: disable=use-list-literal FIXME + 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 - + self.backends = [ + ROS2Backend(self.vehicle_name) + ] # pylint: disable=use-list-literal class AnymalC(QuadrupedRobot): - """AnymalC class - It is a child class of QuadrupedRobot class to implement a AnymalC robot in the simulator. - """ + """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()): @@ -61,7 +70,14 @@ def __init__(self, id: int, init_pos, init_orientation, config=AnymalCConfig()): 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) + super().__init__( + config.stage_prefix, + config.usd_file, + id, + init_pos, + init_orientation, + config=config, + ) self.controller = AnyamlController() @@ -79,7 +95,7 @@ def update_sensors(self, dt: float): for sensor in self._sensors: try: sensor_data = sensor.update(self._state, dt) - except Exception as e: # pylint: disable=broad-except + except Exception as e: # pylint: disable=broad-except print(f"Error updating sensor: {e}") continue diff --git a/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/quadrupedrobot.py b/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/quadrupedrobot.py index 24575f6..e3e39f6 100644 --- a/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/quadrupedrobot.py +++ b/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/quadrupedrobot.py @@ -3,7 +3,11 @@ from stride.simulator.interfaces.stride_sim_interface import StrideInterface import omni -from omni.isaac.core.utils.rotations import quat_to_rot_matrix, quat_to_euler_angles, euler_to_rot_matrix +from omni.isaac.core.utils.rotations import ( + quat_to_rot_matrix, + quat_to_euler_angles, + euler_to_rot_matrix, +) from pxr import Gf import numpy as np import asyncio @@ -37,19 +41,18 @@ def __init__(self): class QuadrupedRobot(Vehicle): - """QuadrupedRobot class - It defines a base interface for creating a QuadrupedRobot - """ + """QuadrupedRobot class - It defines a base interface for creating a QuadrupedRobot""" def __init__( # pylint: disable=dangerous-default-value FIXME - self, - # Simulation specific configurations - stage_prefix: str = "quadrupedrobot", - usd_file: str = "", - vehicle_id: int = 0, - # Spawning pose of the vehicle - init_pos=[0.0, 0.0, 0.07], - init_orientation=[0.0, 0.0, 0.0, 1.0], - config=QuadrupedRobotConfig(), + self, + # Simulation specific configurations + stage_prefix: str = "quadrupedrobot", + usd_file: str = "", + vehicle_id: int = 0, + # Spawning pose of the vehicle + init_pos=[0.0, 0.0, 0.07], + init_orientation=[0.0, 0.0, 0.0, 1.0], + config=QuadrupedRobotConfig(), ): """Initializes the quadrupedrobot object @@ -71,14 +74,18 @@ def __init__( # pylint: disable=dangerous-default-value FIXME # 2. Initialize all the vehicle sensors self._sensors = config.sensors for sensor in self._sensors: - sensor.set_spherical_coordinate(StrideInterface().latitude, - StrideInterface().longitude, - StrideInterface().altitude) + sensor.set_spherical_coordinate( + StrideInterface().latitude, + StrideInterface().longitude, + StrideInterface().altitude, + ) pass # 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 - self._world.add_physics_callback(self._stage_prefix + "/Sensors", self.update_sensors) + self._world.add_physics_callback( + self._stage_prefix + "/Sensors", self.update_sensors + ) # 4. Save the backend interface (if given in the configuration of the vehicle) # and initialize them @@ -87,7 +94,9 @@ def __init__( # pylint: disable=dangerous-default-value FIXME backend.initialize(self) # Add a callback to the physics engine to update the state of the vehicle at every timestep. - self._world.add_physics_callback(self._stage_prefix + "/sim_state", self.update_sim_state) + self._world.add_physics_callback( + self._stage_prefix + "/sim_state", self.update_sim_state + ) # Height scanner y = np.arange(-0.5, 0.6, 0.1) @@ -203,15 +212,21 @@ def _compute_observation(self, command): np.ndarray -- The observation vector. """ - lin_vel_I = self.state.linear_velocity #pylint: disable=invalid-name - ang_vel_I = self.state.angular_velocity #pylint: disable=invalid-name - pos_IB = self.state.position #pylint: disable=invalid-name + lin_vel_I = self.state.linear_velocity # pylint: disable=invalid-name + ang_vel_I = self.state.angular_velocity # pylint: disable=invalid-name + pos_IB = self.state.position # pylint: disable=invalid-name # Convert quaternion from XYZW to WXYZ format - q_IB = np.array( #pylint: disable=invalid-name - [self.state.attitude[-1], self.state.attitude[0], self.state.attitude[1], self.state.attitude[2]]) - - R_IB = quat_to_rot_matrix(q_IB) #pylint: disable=invalid-name - R_BI = R_IB.transpose() #pylint: disable=invalid-name + q_IB = np.array( # pylint: disable=invalid-name + [ + self.state.attitude[-1], + self.state.attitude[0], + self.state.attitude[1], + self.state.attitude[2], + ] + ) + + R_IB = quat_to_rot_matrix(q_IB) # pylint: disable=invalid-name + R_BI = R_IB.transpose() # pylint: disable=invalid-name lin_vel_b = np.matmul(R_BI, lin_vel_I) ang_vel_b = np.matmul(R_BI, ang_vel_I) gravity_b = np.matmul(R_BI, np.array([0.0, 0.0, -1.0])) @@ -243,7 +258,9 @@ def _compute_observation(self, command): current_joint_vel = self.state.joint_velocities current_joint_pos = np.array(current_joint_pos.reshape([3, 4]).T.flat) current_joint_vel = np.array(current_joint_vel.reshape([3, 4]).T.flat) - obs[12:24] = self.controller.joint_pos_scale * (current_joint_pos - self.controller.default_joint_pos) + obs[12:24] = self.controller.joint_pos_scale * ( + current_joint_pos - self.controller.default_joint_pos + ) obs[24:36] = self.controller.joint_vel_scale * current_joint_vel obs[36:48] = self.controller.previous_action @@ -257,8 +274,12 @@ def _compute_observation(self, command): for i in range(world_scan_points.shape[0]): self._query_info.clear() - self.physx_query_interface.raycast_all(tuple(world_scan_points[i]), (0.0, 0.0, -1.0), 100, - self._hit_report_callback) + self.physx_query_interface.raycast_all( + tuple(world_scan_points[i]), + (0.0, 0.0, -1.0), + 100, + self._hit_report_callback, + ) if self._query_info: distance = min(self._query_info) obs[48 + i] = np.clip(distance - 0.5, -1.0, 1.0) diff --git a/exts/stride.simulator/stride/simulator/vehicles/sensors/geo_mag_utils.py b/exts/stride.simulator/stride/simulator/vehicles/sensors/geo_mag_utils.py index 6a5e921..440ed7c 100644 --- a/exts/stride.simulator/stride/simulator/vehicles/sensors/geo_mag_utils.py +++ b/exts/stride.simulator/stride/simulator/vehicles/sensors/geo_mag_utils.py @@ -8,7 +8,13 @@ import numpy as np # Declare which functions are visible from this file -__all__ = ["get_mag_declination", "get_mag_inclination", "get_mag_strength", "reprojection", "GRAVITY_VECTOR"] +__all__ = [ + "get_mag_declination", + "get_mag_inclination", + "get_mag_strength", + "reprojection", + "GRAVITY_VECTOR", +] # -------------------------------------------------------------------- # Magnetic field data from WMM2018 (10^5xnanoTesla (N, E D) n-frame ) @@ -17,54 +23,1539 @@ # Declination data in degrees # pylint: disable=line-too-long, for the sake of readability. DECLINATION_TABLE = [ - [ 47,46,45,43,42,41,39,37,33,29,23,16,10,4,-1,-6,-10,-15,-20,-27,-34,-42,-49,-56,-62,-67,-72,-74,-75,-73,-61,-22,26,42,47,48,47 ], - [ 31,31,31,30,30,30,30,29,27,24,18,11,3,-4,-9,-13,-15,-18,-21,-27,-33,-40,-47,-52,-56,-57,-56,-52,-44,-30,-14,2,14,22,27,30,31 ], - [ 22,23,23,23,22,22,22,23,22,19,13,5,-4,-12,-17,-20,-22,-22,-23,-25,-30,-36,-41,-45,-46,-44,-39,-31,-21,-11,-3,4,10,15,19,21,22 ], - [ 17,17,17,18,17,17,17,17,16,13,8,-1,-10,-18,-22,-25,-26,-25,-22,-20,-21,-25,-29,-32,-31,-28,-23,-16,-9,-3,0,4,7,11,14,16,17 ], - [ 13,13,14,14,14,13,13,12,11,9,3,-5,-14,-20,-24,-25,-24,-21,-17,-12,-9,-11,-14,-17,-18,-16,-12,-8,-3,-0,1,3,6,8,11,12,13 ], - [ 11,11,11,11,11,10,10,10,9,6,-0,-8,-15,-21,-23,-22,-19,-15,-10,-5,-2,-2,-4,-7,-9,-8,-7,-4,-1,1,1,2,4,7,9,10,11 ], - [ 10,9,9,9,9,9,9,8,7,3,-3,-10,-16,-20,-20,-18,-14,-9,-5,-2,1,2,0,-2,-4,-4,-3,-2,-0,0,0,1,3,5,7,9,10 ], - [ 9,9,9,9,9,9,9,8,6,1,-4,-11,-16,-18,-17,-14,-10,-5,-2,-0,2,3,2,0,-1,-2,-2,-1,-0,-1,-1,-1,1,3,6,8,9 ], - [ 8,9,9,10,10,10,10,8,5,0,-6,-12,-15,-16,-15,-11,-7,-4,-1,1,3,4,3,2,1,0,-0,-0,-1,-2,-3,-4,-2,0,3,6,8 ], - [ 7,9,10,11,12,12,12,9,5,-1,-7,-13,-15,-15,-13,-10,-6,-3,0,2,3,4,4,4,3,2,1,0,-1,-3,-5,-6,-6,-3,0,4,7 ], - [ 5,8,11,13,14,15,14,11,5,-2,-9,-15,-17,-16,-13,-10,-6,-3,0,3,4,5,6,6,6,5,4,2,-1,-5,-8,-9,-9,-6,-3,1,5 ], - [ 3,8,11,15,17,17,16,12,5,-4,-12,-18,-19,-18,-16,-12,-8,-4,-0,3,5,7,9,10,10,9,7,4,-1,-6,-10,-12,-12,-9,-5,-1,3 ], - [ 3,8,12,16,19,20,18,13,4,-8,-18,-24,-25,-23,-20,-16,-11,-6,-1,3,7,11,14,16,17,17,14,8,-0,-8,-13,-15,-14,-11,-7,-2,3 ]] + [ + 47, + 46, + 45, + 43, + 42, + 41, + 39, + 37, + 33, + 29, + 23, + 16, + 10, + 4, + -1, + -6, + -10, + -15, + -20, + -27, + -34, + -42, + -49, + -56, + -62, + -67, + -72, + -74, + -75, + -73, + -61, + -22, + 26, + 42, + 47, + 48, + 47, + ], + [ + 31, + 31, + 31, + 30, + 30, + 30, + 30, + 29, + 27, + 24, + 18, + 11, + 3, + -4, + -9, + -13, + -15, + -18, + -21, + -27, + -33, + -40, + -47, + -52, + -56, + -57, + -56, + -52, + -44, + -30, + -14, + 2, + 14, + 22, + 27, + 30, + 31, + ], + [ + 22, + 23, + 23, + 23, + 22, + 22, + 22, + 23, + 22, + 19, + 13, + 5, + -4, + -12, + -17, + -20, + -22, + -22, + -23, + -25, + -30, + -36, + -41, + -45, + -46, + -44, + -39, + -31, + -21, + -11, + -3, + 4, + 10, + 15, + 19, + 21, + 22, + ], + [ + 17, + 17, + 17, + 18, + 17, + 17, + 17, + 17, + 16, + 13, + 8, + -1, + -10, + -18, + -22, + -25, + -26, + -25, + -22, + -20, + -21, + -25, + -29, + -32, + -31, + -28, + -23, + -16, + -9, + -3, + 0, + 4, + 7, + 11, + 14, + 16, + 17, + ], + [ + 13, + 13, + 14, + 14, + 14, + 13, + 13, + 12, + 11, + 9, + 3, + -5, + -14, + -20, + -24, + -25, + -24, + -21, + -17, + -12, + -9, + -11, + -14, + -17, + -18, + -16, + -12, + -8, + -3, + -0, + 1, + 3, + 6, + 8, + 11, + 12, + 13, + ], + [ + 11, + 11, + 11, + 11, + 11, + 10, + 10, + 10, + 9, + 6, + -0, + -8, + -15, + -21, + -23, + -22, + -19, + -15, + -10, + -5, + -2, + -2, + -4, + -7, + -9, + -8, + -7, + -4, + -1, + 1, + 1, + 2, + 4, + 7, + 9, + 10, + 11, + ], + [ + 10, + 9, + 9, + 9, + 9, + 9, + 9, + 8, + 7, + 3, + -3, + -10, + -16, + -20, + -20, + -18, + -14, + -9, + -5, + -2, + 1, + 2, + 0, + -2, + -4, + -4, + -3, + -2, + -0, + 0, + 0, + 1, + 3, + 5, + 7, + 9, + 10, + ], + [ + 9, + 9, + 9, + 9, + 9, + 9, + 9, + 8, + 6, + 1, + -4, + -11, + -16, + -18, + -17, + -14, + -10, + -5, + -2, + -0, + 2, + 3, + 2, + 0, + -1, + -2, + -2, + -1, + -0, + -1, + -1, + -1, + 1, + 3, + 6, + 8, + 9, + ], + [ + 8, + 9, + 9, + 10, + 10, + 10, + 10, + 8, + 5, + 0, + -6, + -12, + -15, + -16, + -15, + -11, + -7, + -4, + -1, + 1, + 3, + 4, + 3, + 2, + 1, + 0, + -0, + -0, + -1, + -2, + -3, + -4, + -2, + 0, + 3, + 6, + 8, + ], + [ + 7, + 9, + 10, + 11, + 12, + 12, + 12, + 9, + 5, + -1, + -7, + -13, + -15, + -15, + -13, + -10, + -6, + -3, + 0, + 2, + 3, + 4, + 4, + 4, + 3, + 2, + 1, + 0, + -1, + -3, + -5, + -6, + -6, + -3, + 0, + 4, + 7, + ], + [ + 5, + 8, + 11, + 13, + 14, + 15, + 14, + 11, + 5, + -2, + -9, + -15, + -17, + -16, + -13, + -10, + -6, + -3, + 0, + 3, + 4, + 5, + 6, + 6, + 6, + 5, + 4, + 2, + -1, + -5, + -8, + -9, + -9, + -6, + -3, + 1, + 5, + ], + [ + 3, + 8, + 11, + 15, + 17, + 17, + 16, + 12, + 5, + -4, + -12, + -18, + -19, + -18, + -16, + -12, + -8, + -4, + -0, + 3, + 5, + 7, + 9, + 10, + 10, + 9, + 7, + 4, + -1, + -6, + -10, + -12, + -12, + -9, + -5, + -1, + 3, + ], + [ + 3, + 8, + 12, + 16, + 19, + 20, + 18, + 13, + 4, + -8, + -18, + -24, + -25, + -23, + -20, + -16, + -11, + -6, + -1, + 3, + 7, + 11, + 14, + 16, + 17, + 17, + 14, + 8, + -0, + -8, + -13, + -15, + -14, + -11, + -7, + -2, + 3, + ], +] # pylint: disable=line-too-long, for the sake of readability. # Inclination data in degrees # pylint: disable=line-too-long, for the sake of readability. INCLINATION_TABLE = [ - [ -78,-76,-74,-72,-70,-68,-65,-63,-60,-57,-55,-54,-54,-55,-56,-57,-58,-59,-59,-59,-59,-60,-61,-63,-66,-69,-73,-76,-79,-83,-86,-87,-86,-84,-82,-80,-78 ], - [ -72,-70,-68,-66,-64,-62,-60,-57,-54,-51,-49,-48,-49,-51,-55,-58,-60,-61,-61,-61,-60,-60,-61,-63,-66,-69,-72,-76,-78,-80,-81,-80,-79,-77,-76,-74,-72 ], - [ -64,-62,-60,-59,-57,-55,-53,-50,-47,-44,-41,-41,-43,-47,-53,-58,-62,-65,-66,-65,-63,-62,-61,-63,-65,-68,-71,-73,-74,-74,-73,-72,-71,-70,-68,-66,-64 ], - [ -55,-53,-51,-49,-46,-44,-42,-40,-37,-33,-30,-30,-34,-41,-48,-55,-60,-65,-67,-68,-66,-63,-61,-61,-62,-64,-65,-66,-66,-65,-64,-63,-62,-61,-59,-57,-55 ], - [ -42,-40,-37,-35,-33,-30,-28,-25,-22,-18,-15,-16,-22,-31,-40,-48,-55,-59,-62,-63,-61,-58,-55,-53,-53,-54,-55,-55,-54,-53,-51,-51,-50,-49,-47,-45,-42 ], - [ -25,-22,-20,-17,-15,-12,-10,-7,-3,1,3,2,-5,-16,-27,-37,-44,-48,-50,-50,-48,-44,-41,-38,-38,-38,-39,-39,-38,-37,-36,-35,-35,-34,-31,-28,-25 ], - [ -5,-2,1,3,5,8,10,13,16,20,21,19,12,2,-10,-20,-27,-30,-30,-29,-27,-23,-19,-17,-17,-17,-18,-18,-17,-16,-16,-16,-16,-15,-12,-9,-5 ], - [ 15,18,21,22,24,26,29,31,34,36,37,34,28,20,10,2,-3,-5,-5,-4,-2,2,5,7,8,7,7,6,7,7,7,6,5,6,8,11,15 ], - [ 31,34,36,38,39,41,43,46,48,49,49,46,42,36,29,24,20,19,20,21,23,25,28,30,30,30,29,29,29,29,28,27,25,25,26,28,31 ], - [ 43,45,47,49,51,53,55,57,58,59,59,56,53,49,45,42,40,40,40,41,43,44,46,47,47,47,47,47,47,47,46,44,42,41,40,42,43 ], - [ 53,54,56,57,59,61,64,66,67,68,67,65,62,60,57,55,55,54,55,56,57,58,59,59,60,60,60,60,60,60,59,57,55,53,52,52,53 ], - [ 62,63,64,65,67,69,71,73,75,75,74,73,70,68,67,66,65,65,65,66,66,67,68,68,69,70,70,71,71,70,69,67,65,63,62,62,62 ], - [ 71,71,72,73,75,77,78,80,81,81,80,79,77,76,74,73,73,73,73,73,73,74,74,75,76,77,78,78,78,78,77,75,73,72,71,71,71 ]] + [ + -78, + -76, + -74, + -72, + -70, + -68, + -65, + -63, + -60, + -57, + -55, + -54, + -54, + -55, + -56, + -57, + -58, + -59, + -59, + -59, + -59, + -60, + -61, + -63, + -66, + -69, + -73, + -76, + -79, + -83, + -86, + -87, + -86, + -84, + -82, + -80, + -78, + ], + [ + -72, + -70, + -68, + -66, + -64, + -62, + -60, + -57, + -54, + -51, + -49, + -48, + -49, + -51, + -55, + -58, + -60, + -61, + -61, + -61, + -60, + -60, + -61, + -63, + -66, + -69, + -72, + -76, + -78, + -80, + -81, + -80, + -79, + -77, + -76, + -74, + -72, + ], + [ + -64, + -62, + -60, + -59, + -57, + -55, + -53, + -50, + -47, + -44, + -41, + -41, + -43, + -47, + -53, + -58, + -62, + -65, + -66, + -65, + -63, + -62, + -61, + -63, + -65, + -68, + -71, + -73, + -74, + -74, + -73, + -72, + -71, + -70, + -68, + -66, + -64, + ], + [ + -55, + -53, + -51, + -49, + -46, + -44, + -42, + -40, + -37, + -33, + -30, + -30, + -34, + -41, + -48, + -55, + -60, + -65, + -67, + -68, + -66, + -63, + -61, + -61, + -62, + -64, + -65, + -66, + -66, + -65, + -64, + -63, + -62, + -61, + -59, + -57, + -55, + ], + [ + -42, + -40, + -37, + -35, + -33, + -30, + -28, + -25, + -22, + -18, + -15, + -16, + -22, + -31, + -40, + -48, + -55, + -59, + -62, + -63, + -61, + -58, + -55, + -53, + -53, + -54, + -55, + -55, + -54, + -53, + -51, + -51, + -50, + -49, + -47, + -45, + -42, + ], + [ + -25, + -22, + -20, + -17, + -15, + -12, + -10, + -7, + -3, + 1, + 3, + 2, + -5, + -16, + -27, + -37, + -44, + -48, + -50, + -50, + -48, + -44, + -41, + -38, + -38, + -38, + -39, + -39, + -38, + -37, + -36, + -35, + -35, + -34, + -31, + -28, + -25, + ], + [ + -5, + -2, + 1, + 3, + 5, + 8, + 10, + 13, + 16, + 20, + 21, + 19, + 12, + 2, + -10, + -20, + -27, + -30, + -30, + -29, + -27, + -23, + -19, + -17, + -17, + -17, + -18, + -18, + -17, + -16, + -16, + -16, + -16, + -15, + -12, + -9, + -5, + ], + [ + 15, + 18, + 21, + 22, + 24, + 26, + 29, + 31, + 34, + 36, + 37, + 34, + 28, + 20, + 10, + 2, + -3, + -5, + -5, + -4, + -2, + 2, + 5, + 7, + 8, + 7, + 7, + 6, + 7, + 7, + 7, + 6, + 5, + 6, + 8, + 11, + 15, + ], + [ + 31, + 34, + 36, + 38, + 39, + 41, + 43, + 46, + 48, + 49, + 49, + 46, + 42, + 36, + 29, + 24, + 20, + 19, + 20, + 21, + 23, + 25, + 28, + 30, + 30, + 30, + 29, + 29, + 29, + 29, + 28, + 27, + 25, + 25, + 26, + 28, + 31, + ], + [ + 43, + 45, + 47, + 49, + 51, + 53, + 55, + 57, + 58, + 59, + 59, + 56, + 53, + 49, + 45, + 42, + 40, + 40, + 40, + 41, + 43, + 44, + 46, + 47, + 47, + 47, + 47, + 47, + 47, + 47, + 46, + 44, + 42, + 41, + 40, + 42, + 43, + ], + [ + 53, + 54, + 56, + 57, + 59, + 61, + 64, + 66, + 67, + 68, + 67, + 65, + 62, + 60, + 57, + 55, + 55, + 54, + 55, + 56, + 57, + 58, + 59, + 59, + 60, + 60, + 60, + 60, + 60, + 60, + 59, + 57, + 55, + 53, + 52, + 52, + 53, + ], + [ + 62, + 63, + 64, + 65, + 67, + 69, + 71, + 73, + 75, + 75, + 74, + 73, + 70, + 68, + 67, + 66, + 65, + 65, + 65, + 66, + 66, + 67, + 68, + 68, + 69, + 70, + 70, + 71, + 71, + 70, + 69, + 67, + 65, + 63, + 62, + 62, + 62, + ], + [ + 71, + 71, + 72, + 73, + 75, + 77, + 78, + 80, + 81, + 81, + 80, + 79, + 77, + 76, + 74, + 73, + 73, + 73, + 73, + 73, + 73, + 74, + 74, + 75, + 76, + 77, + 78, + 78, + 78, + 78, + 77, + 75, + 73, + 72, + 71, + 71, + 71, + ], +] # pylint: disable=line-too-long, for the sake of readability. # Strength data in centi-Tesla STRENGTH_TABLE = [ - [ 62,60,58,56,54,52,49,46,43,41,38,36,34,32,31,31,30,30,30,31,33,35,38,42,46,51,55,59,62,64,66,67,67,66,65,64,62 ], - [ 59,56,54,52,50,47,44,41,38,35,32,29,28,27,26,26,26,25,25,26,28,30,34,39,44,49,54,58,61,64,65,66,65,64,63,61,59 ], - [ 54,52,49,47,45,42,40,37,34,30,27,25,24,24,24,24,24,24,24,24,25,28,32,37,42,48,52,56,59,61,62,62,62,60,59,56,54 ], - [ 49,47,44,42,40,37,35,33,30,28,25,23,22,23,23,24,25,25,26,26,26,28,31,36,41,46,51,54,56,57,57,57,56,55,53,51,49 ], - [ 43,41,39,37,35,33,32,30,28,26,25,23,23,23,24,25,26,28,29,29,29,30,32,36,40,44,48,51,52,52,51,51,50,49,47,45,43 ], - [ 38,36,35,33,32,31,30,29,28,27,26,25,24,24,25,26,28,30,31,32,32,32,33,35,38,42,44,46,47,46,45,45,44,43,41,40,38 ], - [ 34,33,32,32,31,31,31,30,30,30,29,28,27,27,27,28,29,31,32,33,33,33,34,35,37,39,41,42,43,42,41,40,39,38,36,35,34 ], - [ 33,33,32,32,33,33,34,34,35,35,34,33,32,31,30,30,31,32,33,34,35,35,36,37,38,40,41,42,42,41,40,39,37,36,34,33,33 ], - [ 34,34,34,35,36,37,39,40,41,41,40,39,37,35,35,34,35,35,36,37,38,39,40,41,42,43,44,45,45,45,43,41,39,37,35,34,34 ], - [ 37,37,38,39,41,42,44,46,47,47,46,45,43,41,40,39,39,40,41,41,42,43,45,46,47,48,49,50,50,50,48,46,43,41,39,38,37 ], - [ 42,42,43,44,46,48,50,52,53,53,52,51,49,47,45,45,44,44,45,46,46,47,48,50,51,53,54,55,56,55,54,52,49,46,44,43,42 ], - [ 48,48,49,50,52,53,55,56,57,57,56,55,53,51,50,49,48,48,48,49,49,50,51,53,55,56,58,59,60,60,58,56,54,52,50,49,48 ], - [ 54,54,54,55,56,57,58,58,59,58,58,57,56,54,53,52,51,51,51,51,52,53,54,55,57,58,60,61,62,61,61,59,58,56,55,54,54 ]] + [ + 62, + 60, + 58, + 56, + 54, + 52, + 49, + 46, + 43, + 41, + 38, + 36, + 34, + 32, + 31, + 31, + 30, + 30, + 30, + 31, + 33, + 35, + 38, + 42, + 46, + 51, + 55, + 59, + 62, + 64, + 66, + 67, + 67, + 66, + 65, + 64, + 62, + ], + [ + 59, + 56, + 54, + 52, + 50, + 47, + 44, + 41, + 38, + 35, + 32, + 29, + 28, + 27, + 26, + 26, + 26, + 25, + 25, + 26, + 28, + 30, + 34, + 39, + 44, + 49, + 54, + 58, + 61, + 64, + 65, + 66, + 65, + 64, + 63, + 61, + 59, + ], + [ + 54, + 52, + 49, + 47, + 45, + 42, + 40, + 37, + 34, + 30, + 27, + 25, + 24, + 24, + 24, + 24, + 24, + 24, + 24, + 24, + 25, + 28, + 32, + 37, + 42, + 48, + 52, + 56, + 59, + 61, + 62, + 62, + 62, + 60, + 59, + 56, + 54, + ], + [ + 49, + 47, + 44, + 42, + 40, + 37, + 35, + 33, + 30, + 28, + 25, + 23, + 22, + 23, + 23, + 24, + 25, + 25, + 26, + 26, + 26, + 28, + 31, + 36, + 41, + 46, + 51, + 54, + 56, + 57, + 57, + 57, + 56, + 55, + 53, + 51, + 49, + ], + [ + 43, + 41, + 39, + 37, + 35, + 33, + 32, + 30, + 28, + 26, + 25, + 23, + 23, + 23, + 24, + 25, + 26, + 28, + 29, + 29, + 29, + 30, + 32, + 36, + 40, + 44, + 48, + 51, + 52, + 52, + 51, + 51, + 50, + 49, + 47, + 45, + 43, + ], + [ + 38, + 36, + 35, + 33, + 32, + 31, + 30, + 29, + 28, + 27, + 26, + 25, + 24, + 24, + 25, + 26, + 28, + 30, + 31, + 32, + 32, + 32, + 33, + 35, + 38, + 42, + 44, + 46, + 47, + 46, + 45, + 45, + 44, + 43, + 41, + 40, + 38, + ], + [ + 34, + 33, + 32, + 32, + 31, + 31, + 31, + 30, + 30, + 30, + 29, + 28, + 27, + 27, + 27, + 28, + 29, + 31, + 32, + 33, + 33, + 33, + 34, + 35, + 37, + 39, + 41, + 42, + 43, + 42, + 41, + 40, + 39, + 38, + 36, + 35, + 34, + ], + [ + 33, + 33, + 32, + 32, + 33, + 33, + 34, + 34, + 35, + 35, + 34, + 33, + 32, + 31, + 30, + 30, + 31, + 32, + 33, + 34, + 35, + 35, + 36, + 37, + 38, + 40, + 41, + 42, + 42, + 41, + 40, + 39, + 37, + 36, + 34, + 33, + 33, + ], + [ + 34, + 34, + 34, + 35, + 36, + 37, + 39, + 40, + 41, + 41, + 40, + 39, + 37, + 35, + 35, + 34, + 35, + 35, + 36, + 37, + 38, + 39, + 40, + 41, + 42, + 43, + 44, + 45, + 45, + 45, + 43, + 41, + 39, + 37, + 35, + 34, + 34, + ], + [ + 37, + 37, + 38, + 39, + 41, + 42, + 44, + 46, + 47, + 47, + 46, + 45, + 43, + 41, + 40, + 39, + 39, + 40, + 41, + 41, + 42, + 43, + 45, + 46, + 47, + 48, + 49, + 50, + 50, + 50, + 48, + 46, + 43, + 41, + 39, + 38, + 37, + ], + [ + 42, + 42, + 43, + 44, + 46, + 48, + 50, + 52, + 53, + 53, + 52, + 51, + 49, + 47, + 45, + 45, + 44, + 44, + 45, + 46, + 46, + 47, + 48, + 50, + 51, + 53, + 54, + 55, + 56, + 55, + 54, + 52, + 49, + 46, + 44, + 43, + 42, + ], + [ + 48, + 48, + 49, + 50, + 52, + 53, + 55, + 56, + 57, + 57, + 56, + 55, + 53, + 51, + 50, + 49, + 48, + 48, + 48, + 49, + 49, + 50, + 51, + 53, + 55, + 56, + 58, + 59, + 60, + 60, + 58, + 56, + 54, + 52, + 50, + 49, + 48, + ], + [ + 54, + 54, + 54, + 55, + 56, + 57, + 58, + 58, + 59, + 58, + 58, + 57, + 56, + 54, + 53, + 52, + 51, + 51, + 51, + 51, + 52, + 53, + 54, + 55, + 57, + 58, + 60, + 61, + 62, + 61, + 61, + 59, + 58, + 56, + 55, + 54, + 54, + ], +] SAMPLING_RES = 10.0 SAMPLING_MIN_LAT = -60 # deg @@ -144,8 +1635,13 @@ def reprojection(position: np.ndarray, origin_lat=-999, origin_long=-999): cos_c: float = np.cos(c) if c != 0.0: - latitude_rad = np.arcsin(cos_c * np.sin(origin_lat) + (x_rad * sin_c * np.cos(origin_lat)) / c) - longitude_rad = origin_long + np.arctan2(y_rad * sin_c, c * np.cos(origin_lat) * cos_c - x_rad * np.sin(origin_lat) * sin_c) + latitude_rad = np.arcsin( + cos_c * np.sin(origin_lat) + (x_rad * sin_c * np.cos(origin_lat)) / c + ) + longitude_rad = origin_long + np.arctan2( + y_rad * sin_c, + c * np.cos(origin_lat) * cos_c - x_rad * np.sin(origin_lat) * sin_c, + ) else: latitude_rad = origin_lat longitude_rad = origin_long diff --git a/exts/stride.simulator/stride/simulator/vehicles/sensors/imu.py b/exts/stride.simulator/stride/simulator/vehicles/sensors/imu.py index 10b73c0..a9a674e 100644 --- a/exts/stride.simulator/stride/simulator/vehicles/sensors/imu.py +++ b/exts/stride.simulator/stride/simulator/vehicles/sensors/imu.py @@ -9,8 +9,7 @@ class Imu(Sensor): - """The class that implements the Imu sensor. This class inherits the base class Sensor. - """ + """The class that implements the Imu sensor. This class inherits the base class Sensor.""" def __init__(self, config=None): """Initialize the Imu class @@ -38,10 +37,14 @@ def __init__(self, config=None): if config is None: config = {} else: - assert isinstance(config, dict), "The config parameter must be a dictionary." + assert isinstance( + config, dict + ), "The config parameter must be a dictionary." # Initialize the Super class "object" attributes. - super().__init__(sensor_type="Imu", update_frequency=config.get("update_frequency", 250.0)) + super().__init__( + sensor_type="Imu", update_frequency=config.get("update_frequency", 250.0) + ) # Orientation noise constant. self._orientation_noise: float = 0.0 @@ -49,18 +52,32 @@ def __init__(self, config=None): # Gyroscope noise constants self._gyroscope_bias: np.ndarray = np.zeros((3,)) gyroscope_config = config.get("gyroscope", {}) - self._gyroscope_noise_density = gyroscope_config.get("noise_density", 0.0003393695767766752) - self._gyroscope_random_walk = gyroscope_config.get("random_walk", 3.878509448876288E-05) - 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) + self._gyroscope_noise_density = gyroscope_config.get( + "noise_density", 0.0003393695767766752 + ) + self._gyroscope_random_walk = gyroscope_config.get( + "random_walk", 3.878509448876288e-05 + ) + 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. 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_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) + 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. self._prev_linear_velocity = np.zeros((3,)) @@ -101,7 +118,9 @@ def update(self, state: State, dt: float): sigma_b_g: float = self._gyroscope_random_walk # Compute exact covariance of the process after dt [Maybeck 4-114] - sigma_b_g_d: float = np.sqrt(-sigma_b_g * sigma_b_g * tau_g / 2.0 * (np.exp(-2.0 * dt / tau_g) - 1.0)) + sigma_b_g_d: float = np.sqrt( + -sigma_b_g * sigma_b_g * tau_g / 2.0 * (np.exp(-2.0 * dt / tau_g) - 1.0) + ) # Compute state-transition phi_g_d: float = np.exp(-1.0 / tau_g * dt) @@ -110,8 +129,14 @@ def update(self, state: State, dt: float): angular_velocity: np.ndarray = np.zeros((3,)) for i in range(3): - self._gyroscope_bias[i] = phi_g_d * self._gyroscope_bias[i] + sigma_b_g_d * np.random.randn() - angular_velocity[i] = state.angular_velocity[i] + sigma_g_d * np.random.randn() + self._gyroscope_bias[i] + self._gyroscope_bias[i] = ( + phi_g_d * self._gyroscope_bias[i] + sigma_b_g_d * np.random.randn() + ) + angular_velocity[i] = ( + state.angular_velocity[i] + + sigma_g_d * np.random.randn() + + self._gyroscope_bias[i] + ) # Accelerometer terms. tau_a: float = self._accelerometer_bias_correlation_time @@ -121,14 +146,18 @@ def update(self, state: State, dt: float): sigma_b_a: float = self._accelerometer_random_walk # Compute exact covariance of the process after dt [Maybeck 4-114]. - sigma_b_a_d: float = np.sqrt(-sigma_b_a * sigma_b_a * tau_a / 2.0 * (np.exp(-2.0 * dt / tau_a) - 1.0)) + sigma_b_a_d: float = np.sqrt( + -sigma_b_a * sigma_b_a * tau_a / 2.0 * (np.exp(-2.0 * dt / tau_a) - 1.0) + ) # Compute state-transition. phi_a_d: float = np.exp(-1.0 / tau_a * dt) # Compute the linear acceleration from diferentiating the velocity of the vehicle expressed in the inertial # frame. - linear_acceleration_inertial = (state.linear_velocity - self._prev_linear_velocity) / dt + 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. @@ -136,13 +165,18 @@ def update(self, state: State, dt: float): # 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)) + 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. 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] = (linear_acceleration[i] + sigma_a_d * np.random.randn() - ) #+ self._accelerometer_bias[i] + self._accelerometer_bias[i] = ( + phi_a_d * self._accelerometer_bias[i] + sigma_b_a_d * np.random.rand() + ) + linear_acceleration[i] = ( + linear_acceleration[i] + sigma_a_d * np.random.randn() + ) # + self._accelerometer_bias[i] # TODO - Add small "noisy" to the attitude diff --git a/exts/stride.simulator/stride/simulator/vehicles/sensors/lidar.py b/exts/stride.simulator/stride/simulator/vehicles/sensors/lidar.py index f12b241..f4b8ab6 100644 --- a/exts/stride.simulator/stride/simulator/vehicles/sensors/lidar.py +++ b/exts/stride.simulator/stride/simulator/vehicles/sensors/lidar.py @@ -5,7 +5,10 @@ from omni.isaac.core import World from omni.isaac.sensor import RotatingLidarPhysX -from omni.isaac.range_sensor import _range_sensor # Imports the python bindings to interact with lidar sensor +from omni.isaac.range_sensor import ( + _range_sensor, +) # Imports the python bindings to interact with lidar sensor + class Lidar(Sensor): """ @@ -21,22 +24,28 @@ def __init__(self, config=None): if config is None: config = {} else: - assert isinstance(config, dict), "The config parameter must be a dictionary." + assert isinstance( + config, dict + ), "The config parameter must be a dictionary." self.config = config - super().__init__(sensor_type="Lidar", update_frequency=self.config.get("update_frequency", 10.0)) + super().__init__( + sensor_type="Lidar", + update_frequency=self.config.get("update_frequency", 10.0), + ) # Save the state of the sensor self._state = { "points": [], } - self.lidar_interface = _range_sensor.acquire_lidar_sensor_interface() # Used to interact with the LIDAR + self.lidar_interface = ( + _range_sensor.acquire_lidar_sensor_interface() + ) # Used to interact with the LIDAR self.lidar_flag_ = False - @property def state(self): return self._state @@ -45,7 +54,9 @@ def state(self): def update(self, state: State, dt: float): if self.lidar_flag_ is True: - pointcloud = self.lidar_interface.get_point_cloud_data(self.config.get("prim_path")) + pointcloud = self.lidar_interface.get_point_cloud_data( + self.config.get("prim_path") + ) # print(pointcloud) self._state["points"] = pointcloud @@ -57,18 +68,19 @@ def update(self, state: State, dt: float): pass else: self._lidar = my_world.scene.add( - RotatingLidarPhysX(prim_path=self.config.get("prim_path"), name="range_sensor", - rotation_dt = 10 - ) + RotatingLidarPhysX( + prim_path=self.config.get("prim_path"), + name="range_sensor", + rotation_dt=10, + ) ) self._lidar.set_fov([360, 30]) 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) + self._lidar.enable_visualization( + high_lod=True, draw_points=True, draw_lines=False + ) self.lidar_flag_ = True return self._state - \ No newline at end of file diff --git a/exts/stride.simulator/stride/simulator/vehicles/sensors/sensor.py b/exts/stride.simulator/stride/simulator/vehicles/sensors/sensor.py index 28b35d8..59431b4 100644 --- a/exts/stride.simulator/stride/simulator/vehicles/sensors/sensor.py +++ b/exts/stride.simulator/stride/simulator/vehicles/sensors/sensor.py @@ -35,7 +35,9 @@ def __init__(self, sensor_type: str, update_frequency: float): self._origin_longitude = -999 self._origin_altitude = 0.0 - def set_spherical_coordinate(self, origin_latitude, origin_longitude, origin_altitude): + def set_spherical_coordinate( + self, origin_latitude, origin_longitude, origin_altitude + ): """Method that initializes the sensor shperical coordinate attributes. Note: @@ -90,7 +92,9 @@ def wrapper(self, state: State, dt: float): if self.total_time >= self.update_period or self.first_update: # Result of the update function for the sensor. - result = fnc(self, state, self.total_time) # pylint: disable=not-callable, TODO: enable this. + result = fnc( # pylint: disable=not-callable, TODO: enable this. + self, state, self.total_time + ) # Reset the auxiliary counter variables. self.first_update = False diff --git a/exts/stride.simulator/stride/simulator/vehicles/state.py b/exts/stride.simulator/stride/simulator/vehicles/state.py index b9866f0..42b94b4 100644 --- a/exts/stride.simulator/stride/simulator/vehicles/state.py +++ b/exts/stride.simulator/stride/simulator/vehicles/state.py @@ -54,10 +54,14 @@ def __init__(self): self.linear_acceleration = np.array([0.0, 0.0, 0.0]) # The joint angles - self.joint_angles = np.zeros(12) #FIXME: need to be generalized for any number of joints + self.joint_angles = np.zeros( + 12 + ) # FIXME: need to be generalized for any number of joints # The joint velocities - self.joint_velocities = np.zeros(12) #FIXME: need to be generalized for any number of joints + self.joint_velocities = np.zeros( + 12 + ) # FIXME: need to be generalized for any number of joints def get_position_ned(self): """ @@ -80,7 +84,9 @@ def get_attitude_ned_frd(self): the attitude of the vehicle's FRD body frame, relative to an NED inertial frame, expressed in the NED inertial frame. """ - attitude_frd_ned = rot_ENU_to_NED * Rotation.from_quat(self.attitude) * rot_FLU_to_FRD + attitude_frd_ned = ( + rot_ENU_to_NED * Rotation.from_quat(self.attitude) * rot_FLU_to_FRD + ) return attitude_frd_ned.as_quat() def get_linear_body_velocity_ned_frd(self): @@ -95,7 +101,9 @@ def get_linear_body_velocity_ned_frd(self): """ # Get the linear acceleration in FLU convention - linear_acc_body_flu = Rotation.from_quat(self.attitude).inv().apply(self.linear_acceleration) + linear_acc_body_flu = ( + Rotation.from_quat(self.attitude).inv().apply(self.linear_acceleration) + ) # Convert the linear acceleration in the body frame expressed in FLU convention to the FRD convention return rot_FLU_to_FRD.apply(linear_acc_body_flu) diff --git a/exts/stride.simulator/stride/simulator/vehicles/vehicle.py b/exts/stride.simulator/stride/simulator/vehicles/vehicle.py index 8a12031..3449f1b 100644 --- a/exts/stride.simulator/stride/simulator/vehicles/vehicle.py +++ b/exts/stride.simulator/stride/simulator/vehicles/vehicle.py @@ -66,7 +66,9 @@ def __init__( # pylint: disable=dangerous-default-value FIXME # Save the name with which the vehicle will appear in the stage # and the name of the .usd file that contains its description - self._stage_prefix = get_stage_next_free_path(self._current_stage, stage_prefix, False) + self._stage_prefix = get_stage_next_free_path( + self._current_stage, stage_prefix, False + ) self._usd_file = usd_path # Spawn the vehicle primitive in the world's stage @@ -85,7 +87,12 @@ def __init__( # pylint: disable=dangerous-default-value FIXME prim_path=self._stage_prefix, name=self._stage_prefix, position=init_pos, - orientation=[init_orientation[3], init_orientation[0], init_orientation[1], init_orientation[2]], + orientation=[ + init_orientation[3], + init_orientation[0], + init_orientation[1], + init_orientation[2], + ], articulation_controller=None, ) @@ -101,7 +108,9 @@ def __init__( # pylint: disable=dangerous-default-value FIXME self._state = State() # Add a callback to the physics engine to update the current state of the system - self._world.add_physics_callback(self._stage_prefix + "/state", self.update_state) + self._world.add_physics_callback( + self._stage_prefix + "/state", self.update_state + ) # Add the update method to the physics callback if the world was received # so that we can apply forces and torques to the vehicle. Note, this method should @@ -112,8 +121,12 @@ def __init__( # pylint: disable=dangerous-default-value FIXME self._sim_running = False # Add a callback to start/stop of the simulation once the play/stop button is hit - self._world.add_timeline_callback(self._stage_prefix + "/start_sim", self.sim_start) - self._world.add_timeline_callback(self._stage_prefix + "/stop_sim", self.sim_stop) + self._world.add_timeline_callback( + self._stage_prefix + "/start_sim", self.sim_start + ) + self._world.add_timeline_callback( + self._stage_prefix + "/stop_sim", self.sim_stop + ) def __del__(self): """ @@ -201,18 +214,29 @@ def update_state(self, dt: float): # Get the quaternion according in the [qx,qy,qz,qw] standard self._state.attitude = np.array( - [rotation_quat_img[0], rotation_quat_img[1], rotation_quat_img[2], rotation_quat_real]) + [ + rotation_quat_img[0], + rotation_quat_img[1], + rotation_quat_img[2], + rotation_quat_real, + ] + ) # Express the velocity of the vehicle in the inertial frame X_dot = [x_dot, y_dot, z_dot] self._state.linear_velocity = np.array(linear_vel) # The linear velocity V =[u,v,w] of the vehicle's body frame expressed in the body frame of reference # Note that: x_dot = Rot * V - self._state.linear_body_velocity = (Rotation.from_quat(self._state.attitude).inv().apply( - self._state.linear_velocity)) + self._state.linear_body_velocity = ( + Rotation.from_quat(self._state.attitude) + .inv() + .apply(self._state.linear_velocity) + ) # omega = [p,q,r] - self._state.angular_velocity = Rotation.from_quat(self._state.attitude).inv().apply(np.array(ang_vel)) + self._state.angular_velocity = ( + Rotation.from_quat(self._state.attitude).inv().apply(np.array(ang_vel)) + ) # The acceleration of the vehicle expressed in the inertial frame X_ddot = [x_ddot, y_ddot, z_ddot] self._state.linear_acceleration = linear_acceleration diff --git a/exts/stride.simulator/stride/simulator/vehicles/vehicle_manager.py b/exts/stride.simulator/stride/simulator/vehicles/vehicle_manager.py index bb869aa..eef5c2d 100644 --- a/exts/stride.simulator/stride/simulator/vehicles/vehicle_manager.py +++ b/exts/stride.simulator/stride/simulator/vehicles/vehicle_manager.py @@ -101,7 +101,9 @@ def __new__(cls): if cls._instance is None: cls._instance = object.__new__(cls) else: - carb.log_info("Vehicle Manager is defined already, returning the previously defined one") + carb.log_info( + "Vehicle Manager is defined already, returning the previously defined one" + ) return VehicleManager._instance