diff --git a/examples/basic_usage_px4.py b/examples/basic_usage_px4.py new file mode 100644 index 0000000..d059371 --- /dev/null +++ b/examples/basic_usage_px4.py @@ -0,0 +1,42 @@ +# test_px4_sitl.py + +from rotorpy.environments import Environment +from rotorpy.trajectories.circular_traj import ThreeDCircularTraj +from rotorpy.vehicles.px4_multirotor import PX4Multirotor +from rotorpy.vehicles.px4_sihsim_quadx_params import quad_params as sihsim_quadx +from rotorpy.controllers.quadrotor_control import SE3Control +from rotorpy.trajectories.hover_traj import HoverTraj + +import numpy as np +# 1. Make sure you run px4 sitl `PX4_SIMULATOR=none PX4_SIM_MODEL=quadx make px4_sitl sihsim_quadx` in a separate terminal +# 2. Run this example +# - python examples/basic_usage_px4.py + +circular_trajectory = ThreeDCircularTraj(radius=np.array([1,1,0])) +hover_trajectory = HoverTraj(x0=np.array([0, 0, 5])) + +def main(): + vehicle = PX4Multirotor(sihsim_quadx, enable_ground=True) + controller = SE3Control(sihsim_quadx) + + env = Environment( + vehicle = vehicle, + controller = controller, + trajectory = circular_trajectory, + sim_rate = 100, + ) + results = env.run( + t_final = 60, + use_mocap=False, + plot_mocap=False, + plot_estimator=False, + plot_imu=False, + plot = True, + animate_bool = False, + verbose = True, + ) + + print("Done—PX4 SITL ran for", len(results["time"]), "steps") + +if __name__ == '__main__': + main() diff --git a/pyproject.toml b/pyproject.toml index 0d481ed..e70e45e 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,16 +1,16 @@ [project] name = "rotorpy" -version = "2.0.2" +version = "2.1.0" description = "A multirotor simulator with aerodynamics for education and research." readme = "README.md" requires-python = ">=3.8" license = { file = "LICENSE.md" } keywords = ["drone", "uav", "quadrotor", "multirotor", "aerodynamics", "simulation", "controls", "robotics", "estimation"] # Optional authors = [ - { name = "Spencer Folk", email = "sfolk@seas.upenn.edu" } + { name = "Spencer Folk", email = "spencer.folk@gmail.com" } ] maintainers = [ - { name = "Spencer Folk", email = "sfolk@seas.upenn.edu" } + { name = "Spencer Folk", email = "spencer.folk@gmail.com" } ] classifiers = [ "Development Status :: 4 - Beta", @@ -33,7 +33,7 @@ dependencies = [ 'torch>=1.11.0', # For batched sim 'torchdiffeq', # For batched sim 'opt-einsum', # For batched sim - 'timed_count', # Only for ardupilot sitl example + 'timed_count', # Only for ardupilot sitl example ] [project.optional-dependencies] @@ -49,10 +49,21 @@ testing = [ 'pytest', 'filterpy == 1.4.5', 'stable_baselines3', + 'pymavlink' ] filter = [ 'filterpy == 1.4.5', ] +px4 = [ + 'pymavlink', +] +all = [ + "stable_baselines3", + "tensorboard", + "pytest", + "filterpy == 1.4.5", + "pymavlink", +] [project.urls] "Homepage" = "https://github.com/spencerfolk/rotorpy" diff --git a/rotorpy/sensors/imu.py b/rotorpy/sensors/imu.py index 51d8a47..f07148b 100644 --- a/rotorpy/sensors/imu.py +++ b/rotorpy/sensors/imu.py @@ -14,7 +14,7 @@ class Imu: Finishing touches added by Alexander Spinos, checked by Spencer Folk. """ def __init__(self, accelerometer_params={'initial_bias': np.array([0.,0.,0.]), # m/s^2 - 'noise_density': (0.38**2)*np.ones(3,), # m/s^2 / sqrt(Hz) + 'noise_density': (0.038**2)*np.ones(3,), # m/s^2 / sqrt(Hz) 'random_walk': np.zeros(3,) # m/s^2 * sqrt(Hz) }, gyroscope_params={'initial_bias': np.array([0.,0.,0.]), # m/s^2 diff --git a/rotorpy/vehicles/ardupilot_multirotor.py b/rotorpy/vehicles/ardupilot_multirotor.py index 2a94377..06c2abc 100644 --- a/rotorpy/vehicles/ardupilot_multirotor.py +++ b/rotorpy/vehicles/ardupilot_multirotor.py @@ -78,9 +78,6 @@ def step(self, state, control, t_step): if self._ardupilot_control: control = {'cmd_motor_speeds': self._motor_cmd_to_omega(self._control_cmd.cmd_motor_speeds)} - # TODO: this should be moved inside the `Multirotor` class - if self._on_ground(state) and self._enable_ground: - state = self._handle_vehicle_on_ground(state) statedot = self.statedot(state, control, t_step) state = super().step(state, control, t_step) @@ -94,36 +91,6 @@ def step(self, state, control, t_step): return state - def _handle_vehicle_on_ground( - self, state: Dict[str, np.ndarray] - ) -> Dict[str, np.ndarray]: - """ - Handles the vehicle's state when it is on the ground. - This method performs the following actions: - - Constrains the vehicle's position to the ground level (z = 0). - - Stops any downward vertical motion by setting the vertical velocity to zero if it is negative. - - Resets the angular velocity to zero to stop any spinning motion. - - Sets the pitch and roll angles to zero while preserving the heading angle. - Args: - state (Dict[str, np.ndarray]): The current state of the vehicle, which includes position ('x'), - velocity ('v'), angular velocity ('w'), orientation ('q'), - wind vector ('wind') and motor angular velocities ('rotor_speeds'). - Returns: - Dict[str, np.ndarray]: The updated state of the vehicle after applying the ground constraints. - """ - # FIXME: when the motor command is zero and the vehicle is on the ground it drifts (threshold issue?) - state["x"][2] = 0 - - if state["v"][2] < 0: - state["v"] = np.zeros(3,) - - state["w"] = np.zeros( - 3, - ) - - state["q"] = flatten_attitude(state["q"]) - - return state @staticmethod def _motor_cmd_to_omega(pwm_commands : List[int]) -> List[float]: @@ -139,7 +106,35 @@ def _motor_cmd_to_omega(pwm_commands : List[int]) -> List[float]: reordered_pwm_commands = [rotor_2, rotor_0, rotor_3, rotor_1] normalized_commands = [(c-PWM_MIN)/(PWM_MAX-PWM_MIN) for c in reordered_pwm_commands] angular_velocities = [838.0*c for c in normalized_commands] # TODO: remove magic constant - return angular_velocities + return angular_velocities + + @staticmethod + def _quaternion_rotorpy_to_aerospace(quaternion_glu2enu: np.ndarray) -> List[float]: + """ + Convert quaternion from rotorpy convention to aerospace (ArduPilot) convention. + + Args: + quaternion_glu2enu (np.ndarray): Quaternion [x, y, z, w] (scalar-last) representing + rotation from body frame (GLU) to world frame (ENU) + + Returns: + List[float]: Quaternion [w, x, y, z] (scalar-first) representing rotation from + world frame (NED) to body frame (FRD) - aerospace convention + + Notes: + - Input: rotorpy uses scalar-last [x, y, z, w] for GLU→ENU rotation + - Output: ArduPilot uses scalar-first [w, x, y, z] for NED→FRD rotation (inverse) + - Involves coordinate frame transformations: GLU↔FRD and ENU↔NED + """ + # Convert rotorpy quaternion (GLU→ENU) to rotation object + R_glu2enu = R.from_quat(quaternion_glu2enu, scalar_first=False) + + # Transform to aerospace convention (NED→FRD) + # R_frd2ned represents the attitude of the FRD frame in the NED frame + R_frd2ned = Ardupilot.M_enu2ned * R_glu2enu * Ardupilot.M_glu2frd + + # Return as scalar-first quaternion [w, x, y, z] + return R_frd2ned.as_quat(scalar_first=True).tolist() @staticmethod def _create_sensor_data( @@ -167,24 +162,19 @@ def _create_sensor_data( attitude quaternion, and angular velocities. """ - # 1. Obtain attitude quaternion (scalar-first), - # representing the rotation from the body (GLU) frame to the world (ENU) frame - R_glu2enu = R.from_quat(state["q"], scalar_first=False) + # 1. Convert quaternion from rotorpy to aerospace convention + quaternion_aerospace = Ardupilot._quaternion_rotorpy_to_aerospace(state["q"]) - # 2. Obtain the IMU meaurements in the GLU frame + # 2. Obtain the IMU measurements in the GLU frame and transform to FRD acceleration = copy.deepcopy(statedot) meas_dict = imu.measurement(state, acceleration, with_noise=enable_imu_noise) a_glu, omega_glu = meas_dict["accel"], meas_dict["gyro"] a_frd = Ardupilot.M_glu2frd.apply(a_glu).tolist() omega_frd = Ardupilot.M_glu2frd.apply(omega_glu).tolist() - # 2. Obtain the rotation from the body frame (FRD) to the world frame (NED) - # This is the attitude of the FRD frame in the NED frame - R_frd2ned = Ardupilot.M_enu2ned * R_glu2enu * Ardupilot.M_glu2frd - return SensorData( state["x"].tolist(), - R_frd2ned.as_quat(scalar_first=True).tolist(), + quaternion_aerospace, state["v"].tolist(), xgyro=omega_frd[0], ygyro=omega_frd[1], @@ -195,27 +185,8 @@ def _create_sensor_data( ) -def flatten_attitude(quaternion : List[float]) -> List[float]: - """ - Set roll and pitch to 0 while keeping yaw unchanged. - - Parameters: - quaternion (array-like): Quaternion [x, y, z, w] representing the quadrotor's attitude. - - Returns: - numpy.ndarray: New quaternion with roll and pitch set to 0. - """ - - # Extract Euler angles in the 'XYZ' (roll, pitch, heading) convention wrt the world frame - _, _, heading = R.from_quat(quaternion).as_euler('XYZ', degrees=False) - - # Create a new rotation object with roll and pitch set to 0 - flattened_rotation = R.from_euler('Z', heading, degrees=False) - - # Convert the new rotation back to a quaternion - return flattened_rotation.as_quat() - if __name__ == '__main__': + import time r = R.from_euler('y', 0, degrees=True) initial_state = {'x': np.array([0,0,0]), 'v': np.zeros(3,), diff --git a/rotorpy/vehicles/multirotor.py b/rotorpy/vehicles/multirotor.py index e6d47ca..8bc2171 100644 --- a/rotorpy/vehicles/multirotor.py +++ b/rotorpy/vehicles/multirotor.py @@ -1,9 +1,12 @@ +from typing import List import numpy as np from numpy.linalg import inv, norm import scipy.integrate from scipy.spatial.transform import Rotation from rotorpy.vehicles.hummingbird_params import quad_params +from scipy.spatial.transform import Rotation as R + # imports for Batched Dynamics import torch from torchdiffeq import odeint @@ -31,10 +34,7 @@ def quat_dot(quat, omega): [-q2, q3, q0, -q1], [ q1, -q0, q3, -q2]]) quat_dot = 0.5 * G.T @ omega - # Augment to maintain unit quaternion. - quat_err = np.sum(quat**2) - 1 - quat_err_grad = 2 * quat - quat_dot = quat_dot - quat_err * quat_err_grad + # Rely on post-step renormalization instead of a penalty term return quat_dot @@ -81,6 +81,8 @@ class Multirotor(object): 'cmd_vel': the controller commands a velocity vector in the world frame. 'cmd_acc': the controller commands a mass normalized thrust vector (acceleration) in the world frame. aero: boolean, determines whether or not aerodynamic drag forces are computed. + enable_ground: boolean, determines whether or not ground contact is enabled. + integrator_kwargs: dictionary of keyword arguments passed to scipy.integrate.solve_ivp """ def __init__(self, quad_params, initial_state = {'x': np.array([0,0,0]), 'v': np.zeros(3,), @@ -90,7 +92,8 @@ def __init__(self, quad_params, initial_state = {'x': np.array([0,0,0]), 'rotor_speeds': np.array([1788.53, 1788.53, 1788.53, 1788.53])}, control_abstraction='cmd_motor_speeds', aero = True, - enable_ground = False + enable_ground = False, + integrator_kwargs = None, ): """ Initialize quadrotor physical parameters. @@ -150,6 +153,10 @@ def __init__(self, quad_params, initial_state = {'x': np.array([0,0,0]), [0, 0, self.c_Dz]]) self.g = 9.81 # m/s^2 self._enable_ground = enable_ground + # Ground contact horizontal friction (velocity clamp). Beta in [0.1, 0.5]. + self.ground_friction_beta = float(quad_params.get('ground_friction_beta', 0.3)) + # Clamp to a stable range + self.ground_friction_beta = max(0.1, min(0.5, self.ground_friction_beta)) self.inv_inertia = inv(self.inertia) self.weight = np.array([0, 0, -self.mass*self.g]) @@ -169,6 +176,12 @@ def __init__(self, quad_params, initial_state = {'x': np.array([0,0,0]), self.aero = aero + # Integrator settings. + if integrator_kwargs is None: + self.integrator_kwargs = {'method':'RK45'} + else: + self.integrator_kwargs = integrator_kwargs + def extract_geometry(self): """ Extracts the geometry in self.rotors for efficient use later on in the computation of @@ -222,16 +235,24 @@ def s_dot_fn(t, s): return self._s_dot_fn(t, s, cmd_rotor_speeds) s = Multirotor._pack_state(state) - # Option 1 - RK45 integration - sol = scipy.integrate.solve_ivp(s_dot_fn, (0, t_step), s, first_step=t_step) - s = sol['y'][:,-1] - # Option 2 - Euler integration - # s = s + s_dot_fn(0, s) * t_step # first argument doesn't matter. It's time invariant model + # Integrate + sol = scipy.integrate.solve_ivp( + s_dot_fn, + (0.0, t_step), + s, + **self.integrator_kwargs + ) + s = sol['y'][:, -1] + # Unpack the state vector. state = Multirotor._unpack_state(s) # Re-normalize unit quaternion. state['q'] = state['q'] / norm(state['q']) + + # Apply ground constraints (unified across vehicles) + if self._enable_ground and self._on_ground(state): + state = self._handle_vehicle_on_ground(state) # Add noise to the motor speed measurement state['rotor_speeds'] += np.random.normal(scale=np.abs(self.motor_noise), size=(self.num_rotors,)) @@ -271,8 +292,12 @@ def _s_dot_fn(self, t, s, cmd_rotor_speeds): # Rotate the force from the body frame to the inertial frame Ftot = R@FtotB - if self._on_ground(state) and self._enable_ground: - Ftot -= self.weight + # Ground reaction force: apply normal force when on ground to prevent penetration + if self._enable_ground and self._on_ground(state): + total_force_no_ground = self.weight + Ftot + if total_force_no_ground[2] < 0: + ground_normal_force = np.array([0, 0, -total_force_no_ground[2]]) + Ftot += ground_normal_force # Velocity derivative. v_dot = (self.weight + Ftot) / self.mass @@ -459,6 +484,31 @@ def _on_ground(self, state): """ return state['x'][2] <= 0.001 + def _handle_vehicle_on_ground(self, state): + """ + Handle vehicle state while on ground. + - Clamp altitude to ground plane (z = 0) + - Prevent downward motion (v_z >= 0) + - Apply horizontal velocity damping to emulate friction + - Zero angular velocity and flatten attitude (keep yaw) + """ + # Clamp position to ground + state['x'][2] = 0.0 + + # Prevent downward velocity + if state['v'][2] < 0.0: + state['v'][2] = 0.0 + + # Horizontal velocity damping (friction-like) + beta = self.ground_friction_beta + state['v'][0:2] = (1.0 - beta) * state['v'][0:2] + + # Zero angular velocity and flatten attitude (keep yaw) + state['w'] = np.zeros(3,) + state['q'] = self.flatten_attitude(state['q']) + + return state + @classmethod def rotate_k(cls, q): """ @@ -521,6 +571,27 @@ def _unpack_state(cls, s): state = {'x':s[0:3], 'v':s[3:6], 'q':s[6:10], 'w':s[10:13], 'wind':s[13:16], 'rotor_speeds':s[16:]} return state + @staticmethod + def flatten_attitude(quaternion : List[float]) -> List[float]: + """ + Set roll and pitch to 0 while keeping yaw unchanged. + + Parameters: + quaternion (array-like): Quaternion [x, y, z, w] representing the quadrotor's attitude. + + Returns: + numpy.ndarray: New quaternion with roll and pitch set to 0. + """ + + # Extract Euler angles in the 'XYZ' (roll, pitch, heading) convention wrt the world frame + _, _, heading = R.from_quat(quaternion).as_euler('XYZ', degrees=False) + + # Create a new rotation object with roll and pitch set to 0 + flattened_rotation = R.from_euler('Z', heading, degrees=False) + + # Convert the new rotation back to a quaternion + return flattened_rotation.as_quat() + class BatchedMultirotorParams: """ diff --git a/rotorpy/vehicles/px4_multirotor.py b/rotorpy/vehicles/px4_multirotor.py new file mode 100644 index 0000000..50a60a1 --- /dev/null +++ b/rotorpy/vehicles/px4_multirotor.py @@ -0,0 +1,272 @@ +from rotorpy.vehicles.ardupilot_multirotor import Ardupilot +from rotorpy.vehicles.multirotor import Multirotor +from rotorpy.sensors.imu import Imu +from pymavlink import mavutil +import numpy as np +from typing import Tuple + +import math + +# Constants +R_EARTH = 6378137.0 # meters +rad2deg = 180.0 / np.pi +INT_MAX = 32767 +INT_MIN = -32768 + +class SensorSource: + """ The binary codes to signal which simulated data is being sent through mavlink + + Credit to Pegasus Simulator https://github.com/PegasusSimulator/PegasusSimulator + + Atribute: + | ACCEL (int): mavlink binary code for the accelerometer (0b0000000000111 = 7) + | GYRO (int): mavlink binary code for the gyroscope (0b0000000111000 = 56) + | MAG (int): mavlink binary code for the magnetometer (0b0000111000000=448) + | BARO (int): mavlink binary code for the barometer (0b1101000000000=6656) + | DIFF_PRESS (int): mavlink binary code for the pressure sensor (0b0010000000000=1024) + """ + + ACCEL: int = 7 + GYRO: int = 56 + MAG: int = 448 + BARO: int = 6656 + DIFF_PRESS: int = 1024 + +def _compute_hover_rotor_speeds(mass, k_eta, num_rotors, g=9.81): + """Solve N·k_eta·ω² = m·g for ω and return an array length num_rotors.""" + omega = np.sqrt((mass * g) / (num_rotors * k_eta)) + return np.full(num_rotors, omega) + +class PX4Multirotor(Multirotor): + """PX4 Multirotor Vehicle Model + + Args: + quad_params (dict): Quadrotor parameters. + initial_state (dict, optional): Initial state of the quadrotor. + autopilot_controller (bool): Whether to use the autopilot controller or not. + """ + def __init__( + self, + quad_params, + initial_state=None, + control_abstraction="cmd_motor_speeds", + aero=True, + enable_ground=True, + mavlink_url="tcpin:localhost:4560", + autopilot_controller=True, + lockstep=True, + integrator_kwargs=None + ): + integrator_kwargs = integrator_kwargs if integrator_kwargs is not None else {'method':'Radau', 'rtol':1e-3, 'atol':1e-6, 'max_step':0.01} + # If no initial state passed, initialize to hover at origin + if initial_state is None: + initial_state = { + 'x': np.zeros(3), + 'v': np.zeros(3), + 'q': np.array([0, 0, 0, 1]), + 'w': np.zeros(3), + 'wind': np.zeros(3), + 'rotor_speeds': np.zeros(quad_params['num_rotors']) + } + super().__init__( + quad_params=quad_params, + initial_state=initial_state, + control_abstraction=control_abstraction, + aero=aero, + enable_ground=enable_ground, + integrator_kwargs=integrator_kwargs + ) + # Simulated IMU (with noise) + self.imu = Imu() + self._enable_imu_noise = True # Always add a bit of noise to avoid stale detection + self.t = 0.0 + + print("PX4Multirotor: Initializing MAVLink connection... on {}".format(mavlink_url)) + self.conn = mavutil.mavlink_connection(mavlink_url) + self.conn.wait_heartbeat() + print("PX4Multirotor: MAVLink connection established.") + + self._autopilot_controller = autopilot_controller + self._lockstep_enabled = lockstep + + @staticmethod + def enu_to_geodetic( + east_m: float, + north_m: float, + up_m: float, + lat0_deg: float = 40.0, + lon0_deg: float = -74.3, + alt0_m: float = 0.0, + ) -> Tuple[float, float, float]: + """ + Convert local ENU coordinates (meters) to geodetic latitude, longitude, and altitude (WGS-84) + using the simple equirectangular approximation. + + Assumptions: + - Small-area approximation (recommended within ~10–20 km of the reference). + - Spherical Earth with WGS-84 semi-major radius. + + Args: + lat0_deg (float): Reference latitude (degrees, geodetic). + lon0_deg (float): Reference longitude (degrees, geodetic). + alt0_m (float): Reference altitude above mean sea level (meters). + east_m (float): Local ENU 'east' offset from reference (meters). + north_m (float): Local ENU 'north' offset from reference (meters). + up_m (float): Local ENU 'up' offset from reference (meters). + + Returns: + (lat_deg, lon_deg, alt_msl_m): + lat_deg (float): Latitude in degrees (geodetic). + lon_deg (float): Longitude in degrees (geodetic). + alt_msl_m (float): Altitude above mean sea level in meters. + + Notes: + - This keeps longitude unwrapped; you can normalize to [-180, 180) if desired. + - For larger areas or higher accuracy, use a full ENU↔ECEF↔LLA conversion. + """ + # WGS-84 semi-major axis (meters) + R = 6378137.0 + + # Convert reference latitude to radians once + lat0_rad = math.radians(lat0_deg) + + # Equirectangular projection back to geodetic + lat_deg = lat0_deg + (north_m / R) * (180.0 / math.pi) + lon_deg = lon0_deg + (east_m / (R * math.cos(lat0_rad))) * (180.0 / math.pi) + + # Altitude: ENU 'up' increases MSL altitude + alt_msl_m = alt0_m + up_m + + # (Optional) normalize longitude to [-180, 180) + if lon_deg >= 180.0 or lon_deg < -180.0: + lon_deg = ((lon_deg + 180.0) % 360.0) - 180.0 + + return lat_deg, lon_deg, alt_msl_m + + @staticmethod + def geodetic_to_mavlink(lat_deg: float, lon_deg: float, alt_msl_m: float) -> Tuple[int, int, int]: + """ + Convert geodetic coordinates to MAVLink integer fields. + + Args: + lat_deg (float): Latitude in degrees. + lon_deg (float): Longitude in degrees. + alt_msl_m (float): Altitude above mean sea level in meters. + + Returns: + lat_int (int): Latitude in 1E-7 degrees (MAVLink int32). + lon_int (int): Longitude in 1E-7 degrees (MAVLink int32). + alt_mm (int): Altitude above MSL in millimeters (MAVLink int32). + """ + lat_int = int(round(lat_deg * 1e7)) + lon_int = int(round(lon_deg * 1e7)) + alt_mm = int(round(alt_msl_m * 1000.0)) + return lat_int, lon_int, alt_mm + + def _fetch_latest_px4_control(self, blocking : bool = True): + """Fetch the latest HIL_ACTUATOR_CONTROLS message from PX4 and update control inputs.""" + + msg = self.conn.recv_match(type='HIL_ACTUATOR_CONTROLS', blocking=blocking, timeout=0.01) + if msg is not None: + return {'cmd_motor_speeds': [c * self.rotor_speed_max for c in msg.controls[:self.num_rotors]]} + + def _enu_to_ned_cmps(self, v_enu): + v_n = float(v_enu[1]) + v_e = float(v_enu[0]) + v_d = float(-v_enu[2]) + return ( + int(np.round(v_n * 100.0)), + int(np.round(v_e * 100.0)), + int(np.round(v_d * 100.0)), + ) + + def _imu(self, state, statedot): + meas = self.imu.measurement(state, statedot, with_noise=self._enable_imu_noise) + a_flu = meas["accel"] + omega_flu = meas["gyro"] + # FLU -> FRD + a_frd = np.array([a_flu[0], -a_flu[1], -a_flu[2]], dtype=float) + omega_frd = np.array([omega_flu[0], -omega_flu[1], -omega_flu[2]], dtype=float) + return a_frd, omega_frd + + def _send_hil_state_quaternion(self, state, statedot): + """ + Send HIL_STATE_QUATERNION message to PX4. + + Args: + state: Current vehicle state + statedot: State derivative (from the `Multirotor.statedot` method) + """ + # Convert cartesian ENU position to geodetic coordinates (latitude, longitude and height) + lat_deg, lon_deg, height_meters = self.enu_to_geodetic(*state['x']) + lat_e7, lon_e7, alt_mm = self.geodetic_to_mavlink(lat_deg, lon_deg, height_meters) + + # Convert quaternion from rotorpy to aerospace convention using ArduPilot's method + quaternion_flu2ned = Ardupilot._quaternion_rotorpy_to_aerospace(state['q']) + vx_cms, vy_cms, vz_cms = self._enu_to_ned_cmps(state['v']) + + # Send the ground truth acceleration in the state message (without imu noise) + a_flu_gt = self.imu.measurement(state, statedot, with_noise=False)["accel"] + a_frd_gt = np.array([a_flu_gt[0], -a_flu_gt[1], -a_flu_gt[2]], dtype=float) + a_frd_mg = np.clip(np.round(a_frd_gt / 9.80665 * 1000.0), INT_MIN, INT_MAX).astype(np.int16) + + self.conn.mav.hil_state_quaternion_send( + int(self.t * 1e6), + quaternion_flu2ned, + *tuple(state['w']), + lat_e7, lon_e7, alt_mm, + vx_cms, vy_cms, vz_cms, + 0, 0, # Indicated airspeed, true airspeed + int(a_frd_mg[0]), int(a_frd_mg[1]), int(a_frd_mg[2]) + ) + + def _send_hil_sensor(self, state, statedot): + """ + Send HIL_SENSOR message to PX4. + + Args: + state: Current vehicle state + statedot: State derivative (computed externally) + """ + # Get IMU measurements + a_ned, omega_ned = self._imu(state, statedot) + + # Only flag accel/gyro as updated (exclude mag and baro-related fields) + updated_bitmask = SensorSource.ACCEL | SensorSource.GYRO + + self.conn.mav.hil_sensor_send( + int(self.t * 1e6), + *tuple(a_ned), + *tuple(omega_ned), + *(0.0, 0.0, 0.0), # Magnetometer (body frame) + 0.0, # Abs pressure + 0, # Differential pressure + 0.0, # Altitude from pressure + 25, # Temperature + fields_updated=updated_bitmask, + ) + + def step(self, state, control, t_step): + + # Compute state derivative once for state and messages + # and send both HIL messages + statedot = self.statedot(state, control, 0.0) + self._send_hil_state_quaternion(state, statedot) + self._send_hil_sensor(state, statedot) + + # Use PX4 commands only if autopilot_controller is True + if self._autopilot_controller: + px4_control = self._fetch_latest_px4_control(blocking=self._lockstep_enabled) + if px4_control is not None: + control = px4_control + else: + control = {'cmd_motor_speeds': np.zeros(self.num_rotors)} + + else: # In this case we use the control provided by the external controller + pass + + state = super().step(state, control, t_step) + self.state = state + self.t += t_step + + return state diff --git a/rotorpy/vehicles/px4_sihsim_quadx_params.py b/rotorpy/vehicles/px4_sihsim_quadx_params.py new file mode 100644 index 0000000..6f07770 --- /dev/null +++ b/rotorpy/vehicles/px4_sihsim_quadx_params.py @@ -0,0 +1,43 @@ +import numpy as np + +d = 0.17 # distance from CoM to rotor (m) + +# 10040_sihsim_quadx preset (aligned with PX4 SIH parameters) +quad_params = { + 'mass': 1.0, # kg (PX4 param SIH_MASS) + 'Ixx': 0.025, # kg·m² (PX4 param SIH_IXX) + 'Iyy': 0.025, # kg·m² (PX4 param SIH_IYY) + 'Izz': 0.030, # kg·m² (PX4 param SIH_IZZ) + 'Ixy': 0.0, + 'Ixz': 0.0, + 'Iyz': 0.0, + + 'num_rotors': 4, + 'rotor_pos': { + 'r1': d*np.array([ 1.0, -1.0, 0.0]), + 'r2': d*np.array([-1.0, 1.0, 0.0]), + 'r3': d*np.array([ 1.0, 1.0, 0.0]), + 'r4': d*np.array([-1.0, -1.0, 0.0]), + }, + + # Sign for each motor’s yaw moment (+ CW, - CCW) + 'rotor_directions': np.array([ -1, -1, 1, 1 ]), + 'rI': np.array([0.0, 0.0, 0.0]), + + 'c_Dx': 1e-2, + 'c_Dy': 1e-2, + 'c_Dz': 1e-2, + + 'k_eta': 5e-6, # This is computed as SIH_T_MAX/(rotor_speed_max^2) + 'k_m': 1e-7, # max yaw moment per rotor (Nm) to SIH_Q_MAX/(rotor_speed_max^2) + 'k_d': 0.0, # rotor drag + 'k_z': 0.0, # induced inflow + 'k_h': 0.0, # translational lift + 'k_flap': 0.0, # blade flapping moment + + 'tau_m': 0.05, # Motor response time constant (s) from SIH_T_TAU + 'rotor_speed_min': 0.0, # [rad/s] zero throttle + 'rotor_speed_max': 1000.0, # [rad/s] full throttle + 'motor_noise_std': 0.0, +} + diff --git a/tests/test_batched_sims.py b/tests/test_batched_sims.py index ba57fb8..e0ff22f 100644 --- a/tests/test_batched_sims.py +++ b/tests/test_batched_sims.py @@ -71,7 +71,7 @@ def test_batched_operators(): if key == "rotor_speeds": assert np.all(np.abs(batch_next_state[key][j].cpu().numpy() - seq_next_state[key]) < 1) else: - assert np.all(np.abs(batch_next_state[key][j].cpu().numpy() - seq_next_state[key]) < 2e-2) + assert np.all(np.abs(batch_next_state[key][j].cpu().numpy() - seq_next_state[key]) < 3e-2) if __name__ == "__main__":