From 94c05c67e772181ed77124673b1c487c392428d5 Mon Sep 17 00:00:00 2001 From: ratheron Date: Thu, 10 Sep 2026 12:18:15 +0200 Subject: [PATCH 1/6] Add body_rate control --- crazyflow/control/__init__.py | 2 + crazyflow/control/core.py | 6 + crazyflow/control/mellinger/__init__.py | 6 + crazyflow/control/mellinger/control.py | 265 ++++++++++++++++++++- crazyflow/control/mellinger/params.toml | 28 +++ crazyflow/sim/data.py | 31 ++- crazyflow/sim/functional.py | 14 ++ crazyflow/sim/sim.py | 29 ++- docs/examples/index.md | 11 + docs/user-guide/control/controllers.md | 4 +- docs/user-guide/control/index.md | 50 +++- docs/user-guide/control/integral-errors.md | 2 +- docs/user-guide/control/mellinger.md | 43 +++- docs/user-guide/dynamics/index.md | 16 +- docs/user-guide/functional-api.md | 3 +- docs/user-guide/index.md | 2 +- docs/user-guide/oo-api.md | 22 +- tests/integration/test_interfaces.py | 35 +++ tests/unit/control/test_core.py | 8 +- tests/unit/control/test_mellinger.py | 132 ++++++++++ tests/unit/test_sim.py | 13 +- 21 files changed, 685 insertions(+), 37 deletions(-) diff --git a/crazyflow/control/__init__.py b/crazyflow/control/__init__.py index bb651c4b..b1c3e84e 100644 --- a/crazyflow/control/__init__.py +++ b/crazyflow/control/__init__.py @@ -16,11 +16,13 @@ from crazyflow.control.core import Control, load_params, parametrize from crazyflow.control.mellinger import attitude2force_torque as mellinger_attitude2force_torque +from crazyflow.control.mellinger import body_rate2force_torque as mellinger_body_rate2force_torque from crazyflow.control.mellinger import state2attitude as mellinger_state2attitude available_controller: dict[str, Callable] = { "mellinger_state2attitude": mellinger_state2attitude, "mellinger_attitude2force_torque": mellinger_attitude2force_torque, + "mellinger_body_rate2force_torque": mellinger_body_rate2force_torque, } __all__ = ["Control", "load_params", "parametrize"] diff --git a/crazyflow/control/core.py b/crazyflow/control/core.py index 7581c2d8..ccd50dd7 100644 --- a/crazyflow/control/core.py +++ b/crazyflow/control/core.py @@ -100,6 +100,12 @@ class Control(StrEnum): Note: Recommended frequency is >=100 Hz. """ + body_rate = "body_rate" + """Body rate control takes [roll_rate, pitch_rate, yaw_rate, collective thrust]. + + Note: + Recommended frequency is >=200 Hz. + """ force_torque = "force_torque" """Force and torque control takes [fc, tx, ty, tz]. diff --git a/crazyflow/control/mellinger/__init__.py b/crazyflow/control/mellinger/__init__.py index 570613bb..1c18ff4b 100644 --- a/crazyflow/control/mellinger/__init__.py +++ b/crazyflow/control/mellinger/__init__.py @@ -5,10 +5,13 @@ from crazyflow.control.mellinger.control import ( MellingerAttitudeData, + MellingerBodyRateData, MellingerForceTorqueData, MellingerStateData, attitude2force_torque, + body_rate2force_torque, control_attitude2force_torque, + control_body_rate2force_torque, control_commit_attitude, control_force_torque2rotor_vel, control_state2attitude, @@ -19,12 +22,15 @@ __all__ = [ "state2attitude", "attitude2force_torque", + "body_rate2force_torque", "force_torque2rotor_vel", "MellingerStateData", "MellingerAttitudeData", + "MellingerBodyRateData", "MellingerForceTorqueData", "control_state2attitude", "control_attitude2force_torque", + "control_body_rate2force_torque", "control_commit_attitude", "control_force_torque2rotor_vel", ] diff --git a/crazyflow/control/mellinger/control.py b/crazyflow/control/mellinger/control.py index 8b730832..495a8ce6 100644 --- a/crazyflow/control/mellinger/control.py +++ b/crazyflow/control/mellinger/control.py @@ -3,7 +3,8 @@ The controller is split into three pure functions that form a pipeline: ``state2attitude`` → ``attitude2force_torque`` → ``force_torque2rotor_vel``. Each stage can be used independently or chained together to produce per-motor -RPM commands from a full-state setpoint. +RPM commands from a full-state setpoint. ``body_rate2force_torque`` replaces +the second stage for body rate setpoints. Reference: D. Mellinger and V. Kumar, "Minimum snap trajectory generation and control for quadrotors", ICRA 2011. @@ -175,6 +176,8 @@ def attitude2force_torque( quat: Drone orientation as xyzw quaternion with shape (..., 4). ang_vel: Drone angular drone velocity in rad/s with shape (..., 3). cmd: Commanded attitude (roll, pitch, yaw) and total thrust [rad, rad, rad, N]. + prev_ang_vel: Angular velocity in rad/s from the previous call. If None, it is initialised + to zero. r_int_error: Angular velocity integral error (..., 3) from the previous call. If None, it is initialised to zero. ctrl_freq: Control frequency in Hz @@ -187,7 +190,6 @@ def attitude2force_torque( thrust_max: Maximum thrust in N. pwm_min: Minimum PWM value. pwm_max: Maximum PWM value. - prev_ang_vel: Previous angular velocity in rad/s. L: Distance from the center of the quadrotor to the center of the rotor in m. thrust2torque: Conversion factor (m). mixing_matrix: Mixing matrix for the motor forces with shape (4, 3). @@ -196,8 +198,183 @@ def attitude2force_torque( Desired force (1,), torques (3,) and i_error_m """ xp = array_namespace(quat) - force_des = cmd[..., 3] # Total thrust in N - rpy_des = cmd[..., :3] + ang_vel_des = xp.zeros_like(ang_vel) # Attitude control assumes a zero body rate setpoint + return _attitude2force_torque( + quat, + ang_vel, + cmd[..., :3], + ang_vel_des, + cmd[..., 3], + prev_ang_vel, + ang_vel_des, + r_int_error, + ctrl_freq, + kR=kR, + kw=kw, + ki_m=ki_m, + kd_omega=kd_omega, + int_err_max=int_err_max, + torque_pwm_max=torque_pwm_max, + thrust_max=thrust_max, + pwm_min=pwm_min, + pwm_max=pwm_max, + L=L, + thrust2torque=thrust2torque, + mixing_matrix=mixing_matrix, + ) + + +def body_rate2force_torque( + quat: Array, + ang_vel: Array, + cmd: Array, + prev_ang_vel: Array | None = None, + prev_cmd: Array | None = None, + r_int_error: Array | None = None, + ctrl_freq: int = 500, + *, + kR: Array, + kw: Array, + ki_m: Array, + kd_omega: Array, + int_err_max: Array, + torque_pwm_max: Array, + thrust_max: float, + pwm_min: float, + pwm_max: float, + L: float, + thrust2torque: float, + mixing_matrix: Array, +) -> tuple[Array, Array, Array]: + """Compute the body rate to desired force-torque part of the Mellinger controller. + + The firmware Mellinger controller has no dedicated body rate mode. A body rate setpoint enters + the angular velocity error and its derivative, while the attitude terms level the drone at its + current yaw. This function reproduces this behavior with the gains of the attitude controller. + Set ``kR`` and ``ki_m`` to zero to track body rates without the attitude terms. + + Note: + We omit the axis flip in the firmware as it has only been introduced to make the controller + compatible with the new frame of the Crazyflie 2.1. + + Args: + quat: Drone orientation as xyzw quaternion with shape (..., 4). + ang_vel: Drone angular velocity in the body frame in rad/s with shape (..., 3). + cmd: Commanded body rates (wx, wy, wz) and total thrust [rad/s, rad/s, rad/s, N]. + prev_ang_vel: Angular velocity in rad/s from the previous call. If None, it is initialised + to zero. + prev_cmd: Command from the previous call with shape (..., 4). The firmware includes the + derivative of the body rate setpoint in the derivative term. If None, the setpoint is + assumed to be constant. + r_int_error: Angular velocity integral error (..., 3) from the previous call. If None, it + is initialised to zero. + ctrl_freq: Control frequency in Hz + kR: Proportional gain for the rotation error with shape (3,). + kw: Proportional gain for the angular velocity error with shape (3,). + ki_m: Integral gain for the rotation error with shape (3,). + kd_omega: Derivative gain for the angular velocity error with shape (3,). + int_err_max: Range of the integral error with shape (3,). i_range in the firmware. + torque_pwm_max: Maximum torque in PWM. + thrust_max: Maximum thrust in N. + pwm_min: Minimum PWM value. + pwm_max: Maximum PWM value. + L: Distance from the center of the quadrotor to the center of the rotor in m. + thrust2torque: Conversion factor (m). + mixing_matrix: Mixing matrix for the motor forces with shape (4, 3). + + Returns: + Desired force (1,), torques (3,) and i_error_m + """ + xp = array_namespace(quat) + # l. 215 ff Without a position or attitude setpoint, the firmware levels the drone at the + # current yaw + yaw = R.from_quat(quat).as_euler("xyz", degrees=False)[..., 2] + rpy_des = xp.stack((xp.zeros_like(yaw), xp.zeros_like(yaw), yaw), axis=-1) + ang_vel_des = cmd[..., :3] + prev_ang_vel_des = ang_vel_des if prev_cmd is None else prev_cmd[..., :3] + return _attitude2force_torque( + quat, + ang_vel, + rpy_des, + ang_vel_des, + cmd[..., 3], + prev_ang_vel, + prev_ang_vel_des, + r_int_error, + ctrl_freq, + kR=kR, + kw=kw, + ki_m=ki_m, + kd_omega=kd_omega, + int_err_max=int_err_max, + torque_pwm_max=torque_pwm_max, + thrust_max=thrust_max, + pwm_min=pwm_min, + pwm_max=pwm_max, + L=L, + thrust2torque=thrust2torque, + mixing_matrix=mixing_matrix, + ) + + +def _attitude2force_torque( + quat: Array, + ang_vel: Array, + rpy_des: Array, + ang_vel_des: Array, + force_des: Array, + prev_ang_vel: Array | None, + prev_ang_vel_des: Array, + r_int_error: Array | None, + ctrl_freq: int, + *, + kR: Array, + kw: Array, + ki_m: Array, + kd_omega: Array, + int_err_max: Array, + torque_pwm_max: Array, + thrust_max: float, + pwm_min: float, + pwm_max: float, + L: float, + thrust2torque: float, + mixing_matrix: Array, +) -> tuple[Array, Array, Array]: + """Attitude and body rate controller of the Mellinger controller. + + This function follows the structure of the firmware implementation. The firmware setpoint + carries both an attitude and a body rate. The attitude and body rate controllers route their + commands into the respective setpoint. + + Args: + quat: Drone orientation as xyzw quaternion with shape (..., 4). + ang_vel: Drone angular velocity in the body frame in rad/s with shape (..., 3). + rpy_des: Desired attitude as roll, pitch, yaw in rad with shape (..., 3). + ang_vel_des: Desired angular velocity in the body frame in rad/s with shape (..., 3). + force_des: Desired total thrust in N with shape (...,). + prev_ang_vel: Angular velocity from the previous call. If None, it is initialised to zero. + prev_ang_vel_des: Desired angular velocity from the previous call with shape (..., 3). + r_int_error: Rotation integral error (..., 3) from the previous call. If None, it is + initialised to zero. + ctrl_freq: Control frequency in Hz + kR: Proportional gain for the rotation error with shape (3,). + kw: Proportional gain for the angular velocity error with shape (3,). + ki_m: Integral gain for the rotation error with shape (3,). + kd_omega: Derivative gain for the angular velocity error with shape (3,). + int_err_max: Range of the integral error with shape (3,). i_range in the firmware. + torque_pwm_max: Maximum torque in PWM. + thrust_max: Maximum thrust in N. + pwm_min: Minimum PWM value. + pwm_max: Maximum PWM value. + L: Distance from the center of the quadrotor to the center of the rotor in m. + thrust2torque: Conversion factor (m). + mixing_matrix: Mixing matrix for the motor forces with shape (4, 3). + + Returns: + Desired force (..., 1), torques (..., 3) and i_error_m + """ + xp = array_namespace(quat) dt = 1 / ctrl_freq # l. 220 ff [eR]. We're using the "inefficient" code path from the firmware rot = R.from_quat(quat) @@ -210,11 +387,10 @@ def attitude2force_torque( # Vee operator (SO3 to R3) eR = xp.stack((eRM[..., 2, 1], eRM[..., 0, 2], eRM[..., 1, 0]), axis=-1) # l.248 ff [ew] - # Warning: We assume zero desired angular velocity - ang_vel_des = xp.zeros_like(ang_vel) - prev_ang_vel_des = xp.zeros_like(ang_vel) + # The firmware negates the pitch components of the gyro and the rate setpoint to convert them + # to the legacy Crazyflie frame, matching the sign flip of eR.y. We omit both flips and keep all + # terms in the standard body frame, so the setpoint enters without a sign change. ew = ang_vel_des - ang_vel - # WARNING: if the setpoint is ever != 0 => change sign of ew.y! # l.259 ff [err_d_rpy] prev_ang_vel = xp.zeros_like(ang_vel) if prev_ang_vel is None else prev_ang_vel @@ -384,6 +560,46 @@ def create( ) +@dataclass +class MellingerBodyRateData: + cmd: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 4) + """Body rate control command for the drone. + + A command consists of [roll_rate, pitch_rate, yaw_rate, collective thrust]. + """ + staged_cmd: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 4) + """Staging buffer to store the most recent command until the next controller tick.""" + steps: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, 1) + """Last simulation steps that the body rate control command was applied.""" + freq: int = field(pytree_node=False) + """Frequency of the body rate control command.""" + r_int_error: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 3) + """Integral errors of the body rate control command.""" + last_ang_vel: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 3) + """Last angular velocity of the drone.""" + # Parameters for the body rate controller + params: dict[str, Array] + + @staticmethod + def create( + n_worlds: int, n_drones: int, freq: int, drone: str, device: Device + ) -> MellingerBodyRateData: + """Create a default set of body rate data for the simulation.""" + cmd = jnp.zeros((n_worlds, n_drones, 4), device=device) + steps = -jnp.ones((n_worlds, 1), dtype=jnp.int32, device=device) + zeros_3d = jnp.zeros((n_worlds, n_drones, 3), device=device) + params = load_params(body_rate2force_torque, drone, xp=jnp, device=device) + return MellingerBodyRateData( + cmd=cmd, + staged_cmd=cmd, + steps=steps, + freq=freq, + r_int_error=zeros_3d, + last_ang_vel=zeros_3d, + params=params, + ) + + @dataclass class MellingerForceTorqueData: cmd: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 4) @@ -464,6 +680,39 @@ def control_attitude2force_torque(data: SimData) -> SimData: ) +def control_body_rate2force_torque(data: SimData) -> SimData: + """Compute the updated controls for the body rate controller.""" + states = data.states + body_rate_ctrl: MellingerBodyRateData = data.controls.body_rate + assert body_rate_ctrl is not None, "Using body rate controller without initialized data" + mask = controllable(data.core.steps, data.core.freq, body_rate_ctrl.steps, body_rate_ctrl.freq) + prev_cmd = body_rate_ctrl.cmd + body_rate_ctrl = leaf_replace(body_rate_ctrl, mask, cmd=body_rate_ctrl.staged_cmd) + force, torque, r_int_error = body_rate2force_torque( + states.quat, + states.ang_vel, + body_rate_ctrl.cmd, + prev_ang_vel=body_rate_ctrl.last_ang_vel, + prev_cmd=prev_cmd, + r_int_error=body_rate_ctrl.r_int_error, + ctrl_freq=body_rate_ctrl.freq, + **body_rate_ctrl.params, + ) + body_rate_ctrl = leaf_replace( + body_rate_ctrl, + mask, + r_int_error=r_int_error, + last_ang_vel=states.ang_vel, + steps=data.core.steps, + ) + ft_ctrl = leaf_replace( + data.controls.force_torque, mask, staged_cmd=jnp.concat([force, torque], axis=-1) + ) + return data.replace( + controls=data.controls.replace(body_rate=body_rate_ctrl, force_torque=ft_ctrl) + ) + + def control_commit_attitude(data: SimData) -> SimData: """Commit the staged attitude command to the controller setpoint.""" attitude_ctrl: MellingerAttitudeData = data.controls.attitude diff --git a/crazyflow/control/mellinger/params.toml b/crazyflow/control/mellinger/params.toml index 9589be8d..e4270c01 100644 --- a/crazyflow/control/mellinger/params.toml +++ b/crazyflow/control/mellinger/params.toml @@ -31,6 +31,13 @@ ki_m = [0.0, 0.0, 500.0] kd_omega = [200.0, 200.0, 0.0] int_err_max = [1.0, 1.0, 1500.0] +[cf2x_L250.body_rate2force_torque] +kR = [70000.0, 70000.0, 60000.0] +kw = [20000.0, 20000.0, 12000.0] +ki_m = [0.0, 0.0, 500.0] +kd_omega = [200.0, 200.0, 0.0] +int_err_max = [1.0, 1.0, 1500.0] + [cf2x_P250] [cf2x_P250.core] mass = 0.029 # The controller is using the wrong mass by default @@ -64,6 +71,13 @@ ki_m = [0.0, 0.0, 500.0] kd_omega = [200.0, 200.0, 0.0] int_err_max = [1.0, 1.0, 1500.0] +[cf2x_P250.body_rate2force_torque] +kR = [70000.0, 70000.0, 60000.0] +kw = [20000.0, 20000.0, 12000.0] +ki_m = [0.0, 0.0, 500.0] +kd_omega = [200.0, 200.0, 0.0] +int_err_max = [1.0, 1.0, 1500.0] + [cf2x_T350] [cf2x_T350.core] mass = 0.0325 # The controller is using the wrong mass by default @@ -97,6 +111,13 @@ ki_m = [0.0, 0.0, 500.0] kd_omega = [200.0, 200.0, 0.0] int_err_max = [1.0, 1.0, 1500.0] +[cf2x_T350.body_rate2force_torque] +kR = [70000.0, 70000.0, 60000.0] +kw = [20000.0, 20000.0, 12000.0] +ki_m = [0.0, 0.0, 500.0] +kd_omega = [200.0, 200.0, 0.0] +int_err_max = [1.0, 1.0, 1500.0] + [cf21B_500] [cf21B_500.core] gravity_vec = [0.0, 0.0, -9.81] @@ -129,3 +150,10 @@ kw = [20000.0, 20000.0, 12000.0] ki_m = [0.0, 0.0, 500.0] kd_omega = [200.0, 200.0, 0.0] int_err_max = [1.0, 1.0, 1500.0] + +[cf21B_500.body_rate2force_torque] +kR = [70000.0, 70000.0, 60000.0] +kw = [20000.0, 20000.0, 12000.0] +ki_m = [0.0, 0.0, 500.0] +kd_omega = [200.0, 200.0, 0.0] +int_err_max = [1.0, 1.0, 1500.0] diff --git a/crazyflow/sim/data.py b/crazyflow/sim/data.py index f2ea92c1..de918710 100644 --- a/crazyflow/sim/data.py +++ b/crazyflow/sim/data.py @@ -11,6 +11,7 @@ from crazyflow.control import Control from crazyflow.control.mellinger import ( MellingerAttitudeData, + MellingerBodyRateData, MellingerForceTorqueData, MellingerStateData, ) @@ -108,6 +109,8 @@ class SimControls: """State control data.""" attitude: ControlData | None """Attitude control data.""" + body_rate: ControlData | None + """Body rate control data.""" force_torque: ControlData | None """Force and torque control data.""" rotor_vel: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 4) @@ -121,6 +124,7 @@ def create( drone: str, state_freq: int | None, attitude_freq: int | None, + body_rate_freq: int | None, force_torque_freq: int | None, device: Device, ) -> SimControls: @@ -139,11 +143,12 @@ def create( mode=control, state=state, attitude=attitude, + body_rate=None, force_torque=force_torque, rotor_vel=rotor_vel, ) case Control.attitude: - attitude = attitude = MellingerAttitudeData.create( + attitude = MellingerAttitudeData.create( n_worlds, n_drones, attitude_freq, drone, device ) force_torque = MellingerForceTorqueData.create( @@ -153,6 +158,22 @@ def create( mode=control, state=None, attitude=attitude, + body_rate=None, + force_torque=force_torque, + rotor_vel=rotor_vel, + ) + case Control.body_rate: + body_rate = MellingerBodyRateData.create( + n_worlds, n_drones, body_rate_freq, drone, device + ) + force_torque = MellingerForceTorqueData.create( + n_worlds, n_drones, force_torque_freq, drone, device + ) + return SimControls( + mode=control, + state=None, + attitude=None, + body_rate=body_rate, force_torque=force_torque, rotor_vel=rotor_vel, ) @@ -164,12 +185,18 @@ def create( mode=control, state=None, attitude=None, + body_rate=None, force_torque=force_torque, rotor_vel=rotor_vel, ) case Control.rotor_vel: return SimControls( - mode=control, state=None, attitude=None, force_torque=None, rotor_vel=rotor_vel + mode=control, + state=None, + attitude=None, + body_rate=None, + force_torque=None, + rotor_vel=rotor_vel, ) case _: raise ValueError(f"Control mode {control} not implemented") diff --git a/crazyflow/sim/functional.py b/crazyflow/sim/functional.py index e69a769a..7a592f2b 100644 --- a/crazyflow/sim/functional.py +++ b/crazyflow/sim/functional.py @@ -42,6 +42,18 @@ def attitude_control(data: SimData, controls: Array) -> SimData: return data +def body_rate_control(data: SimData, controls: Array) -> SimData: + """Body rate control function.""" + assert data.controls.mode == Control.body_rate, f"control type {data.controls.mode} not enabled" + assert controls.shape == (data.core.n_worlds, data.core.n_drones, 4), "controls shape mismatch" + controls = jnp.asarray(controls) + return data.replace( + controls=data.controls.replace( + body_rate=data.controls.body_rate.replace(staged_cmd=controls) + ) + ) + + def force_torque_control(data: SimData, controls: Array) -> SimData: """Force-torque control function.""" assert data.controls.mode == Control.force_torque, ( @@ -76,6 +88,8 @@ def controllable(data: SimData) -> Array: control_steps, control_freq = controls.state.steps, controls.state.freq case Control.attitude: control_steps, control_freq = controls.attitude.steps, controls.attitude.freq + case Control.body_rate: + control_steps, control_freq = controls.body_rate.steps, controls.body_rate.freq case Control.force_torque: control_steps = controls.force_torque.steps control_freq = controls.force_torque.freq diff --git a/crazyflow/sim/sim.py b/crazyflow/sim/sim.py index c9dd7f2c..e81bedaa 100644 --- a/crazyflow/sim/sim.py +++ b/crazyflow/sim/sim.py @@ -18,6 +18,7 @@ from crazyflow.control import Control from crazyflow.control.mellinger import ( control_attitude2force_torque, + control_body_rate2force_torque, control_commit_attitude, control_force_torque2rotor_vel, control_state2attitude, @@ -81,6 +82,7 @@ def __init__( freq: int = 500, state_freq: int = 100, attitude_freq: int = 500, + body_rate_freq: int = 500, force_torque_freq: int = 500, device: str = "cpu", xml_path: Path | None = None, @@ -99,6 +101,7 @@ def __init__( freq: Dynamics step frequency in Hz. state_freq: Frequency in Hz at which the state controller runs. attitude_freq: Frequency in Hz at which the attitude controller runs. + body_rate_freq: Frequency in Hz at which the body rate controller runs. force_torque_freq: Frequency in Hz at which the force/torque controller runs. device: Device to place the simulation data on (e.g. ``"cpu"`` or ``"gpu"``). xml_path: Path to a custom scene XML. Defaults to ``crazyflow/scene.xml``. @@ -110,7 +113,7 @@ def __init__( assert Dynamics(dynamics) in Dynamics, f"Dynamics mode {dynamics} not implemented" assert Control(control) in Control, f"Control mode {control} not implemented" if dynamics != Dynamics.first_principles: - if control in (Control.force_torque, Control.rotor_vel): + if control in (Control.body_rate, Control.force_torque, Control.rotor_vel): raise ConfigError(f"Control mode {control} requires first principles dynamics") if freq > 10_000 and not jax.config.jax_enable_x64: raise ConfigError("High frequency simulations require double precision mode") @@ -132,7 +135,9 @@ def __init__( self.mj_model, self.mj_data, self.mjx_model, self.mjx_data = self.build_mjx_model(self.spec) self.viewer: MujocoRenderer | None = None - self.data = self.init_data(state_freq, attitude_freq, force_torque_freq, rng_key) + self.data = self.init_data( + state_freq, attitude_freq, body_rate_freq, force_torque_freq, rng_key + ) self.default_data: SimData = self.build_default_data() # Build the simulation pipeline and overwrite the default _step implementation with it @@ -184,6 +189,10 @@ def attitude_control(self, controls: Array): """Set the desired attitude for all drones in all worlds.""" self.data = F.attitude_control(self.data, controls) + def body_rate_control(self, controls: Array): + """Set the desired body rates and collective thrust for all drones in all worlds.""" + self.data = F.body_rate_control(self.data, controls) + def force_torque_control(self, controls: Array): """Set the desired force and torque for all drones in all worlds.""" self.data = F.force_torque_control(self.data, controls) @@ -432,9 +441,10 @@ def build_data(self) -> SimData: """ state_freq = 0 if (s := self.data.controls.state) is None else s.freq attitude_freq = 0 if (a := self.data.controls.attitude) is None else a.freq + body_rate_freq = 0 if (br := self.data.controls.body_rate) is None else br.freq force_torque_freq = 0 if (ft := self.data.controls.force_torque) is None else ft.freq self.data = self.init_data( - state_freq, attitude_freq, force_torque_freq, self.data.core.rng_key + state_freq, attitude_freq, body_rate_freq, force_torque_freq, self.data.core.rng_key ) return self.data @@ -474,7 +484,12 @@ def build_mjx(self): self.mj_model, self.mj_data, self.mjx_model, self.mjx_data = self.build_mjx_model(self.spec) def init_data( - self, state_freq: int, attitude_freq: int, force_torque_freq: int, rng_key: Array + self, + state_freq: int, + attitude_freq: int, + body_rate_freq: int, + force_torque_freq: int, + rng_key: Array, ) -> SimData: """Initialize the simulation data.""" drone_name = "drone_fused" if self.fused_mjx_model else "drone" @@ -492,6 +507,7 @@ def init_data( self.drone, state_freq, attitude_freq, + body_rate_freq, force_torque_freq, self.device, ), @@ -514,6 +530,8 @@ def control_freq(self) -> int: return self.data.controls.state.freq if self.control == Control.attitude: return self.data.controls.attitude.freq + if self.control == Control.body_rate: + return self.data.controls.body_rate.freq if self.control == Control.force_torque: return self.data.controls.force_torque.freq raise NotImplementedError(f"Control mode {self.control} not implemented") @@ -566,6 +584,7 @@ def build_control_fns( """ state = ("state_controller", control_state2attitude) attitude = ("attitude_controller", control_attitude2force_torque) + body_rate = ("body_rate_controller", control_body_rate2force_torque) force_torque = ("force_torque_controller", control_force_torque2rotor_vel) commit_attitude = ("commit_attitude", control_commit_attitude) match control: @@ -580,6 +599,8 @@ def build_control_fns( stages = (commit_attitude,) else: raise NotImplementedError(f"Control mode {control} not implemented for {dynamics}") + case Control.body_rate: + stages = (body_rate, force_torque) case Control.force_torque: stages = (force_torque,) case Control.rotor_vel: diff --git a/docs/examples/index.md b/docs/examples/index.md index 97a133cb..20036a77 100644 --- a/docs/examples/index.md +++ b/docs/examples/index.md @@ -30,6 +30,17 @@ Commanding roll, pitch, yaw, and collective thrust directly. This level bypasses --- +## Body rate control + +Commanding body-frame angular rates and collective thrust. The firmware controller has no dedicated body rate mode and levels the drone with its attitude terms, so the example sets the `kR` and `ki_m` gains of the body rate controller to zero. + + +```{ .python notest } +--8<-- "examples/control/body_rate.py" +``` + +--- + ## Sampling-based MPC A sampling-based model predictive controller tracks a Lissajous curve while avoiding a grid of obstacles. It rolls out thousands of candidate control sequences in parallel using identified dynamics, then applies the first action from a cost-weighted update of the best samples. The controller automatically uses a GPU when one is available and lowers the sample count on CPU. diff --git a/docs/user-guide/control/controllers.md b/docs/user-guide/control/controllers.md index 6c04330d..8904e24a 100644 --- a/docs/user-guide/control/controllers.md +++ b/docs/user-guide/control/controllers.md @@ -16,11 +16,13 @@ The Mellinger controller [[1]](#references) is split into three stages that form | 2 | [`attitude2force_torque`](mellinger.md#attitude-to-force-torque) | Attitude + RPYT command | Collective force, body torques + angular velocity integral error | | 3 | [`force_torque2rotor_vel`](mellinger.md#force-torque-to-rotor-velocities) | Force + torques | 4 motor speeds [RPM] | +[`body_rate2force_torque`](mellinger.md#body-rate-to-force-torque) replaces stage 2 when the command is a body rate setpoint instead of an attitude. It runs the same controller with the rate setpoint in the angular velocity error and a level attitude setpoint, as the firmware does. + ## Available controllers | Module | Controller | Stages | |---|---|---| -| `crazyflow.control.mellinger` | Mellinger | `state2attitude`, `attitude2force_torque`, `force_torque2rotor_vel` | +| `crazyflow.control.mellinger` | Mellinger | `state2attitude`, `attitude2force_torque`, `body_rate2force_torque`, `force_torque2rotor_vel` | ## References diff --git a/docs/user-guide/control/index.md b/docs/user-guide/control/index.md index 6da257b5..26a2392b 100644 --- a/docs/user-guide/control/index.md +++ b/docs/user-guide/control/index.md @@ -1,22 +1,22 @@ # Control Modes -Crazyflow provides four levels of control abstraction, from high-level position setpoints down to direct motor commands. Each level is a separate control mode selected at construction time. +Crazyflow provides five control modes, from high-level position setpoints down to direct motor commands. Each mode is selected at construction time. ## Control hierarchy -Commands flow down a hierarchy. A state command is converted to an attitude command by the Mellinger controller; an attitude command is converted to force/torque by the geometric controller; force/torque is converted to rotor velocities by the mixer. +Commands flow down a hierarchy. A state command is converted to an attitude command by the Mellinger controller; an attitude command is converted to force/torque by the geometric controller; force/torque is converted to rotor velocities by the mixer. Body rate control feeds the geometric controller with a rate setpoint instead of an attitude, so it enters the hierarchy at the same level as attitude control. ``` State (13D) └─ Mellinger controller - └─ Attitude (4D: roll, pitch, yaw, thrust) + └─ Attitude (4D: roll, pitch, yaw, thrust) | Body rates (4D: ωx, ωy, ωz, thrust) └─ Geometric controller └─ Force/torque (4D: Fc, Tx, Ty, Tz) └─ Mixer └─ Rotor velocities (4D: ω₁…ω₄) ``` -When you select `Control.state`, the full chain runs on every control tick. When you select `Control.attitude`, only the lower two stages run. +When you select `Control.state`, the full chain runs on every control tick. When you select `Control.attitude` or `Control.body_rate`, only the lower two stages run. ## State control @@ -94,6 +94,45 @@ sim.attitude_control(cmd) sim.step(sim.freq // sim.control_freq) ``` +## Body rate control + +Commands body-frame angular rates and a collective thrust. The Mellinger controller tracks the rates with the same gains as in attitude control. As in the firmware, its attitude terms level the drone at the current yaw. Set the `kR` and `ki_m` parameters of the body rate controller to zero to track body rates without the levelling terms, see the [body rate example](../../examples/index.md#body-rate-control). Requires `Dynamics.first_principles`. + +```python +from crazyflow.sim import Sim, Dynamics +from crazyflow.control import Control + +sim = Sim(control=Control.body_rate, dynamics=Dynamics.first_principles, body_rate_freq=500) +sim.reset() +``` + +Command shape: `(n_worlds, n_drones, 4)` + +| Index | Variable | Units | +|---|---|---| +| 0 | Roll rate \(\omega_x\) | rad/s | +| 1 | Pitch rate \(\omega_y\) | rad/s | +| 2 | Yaw rate \(\omega_z\) | rad/s | +| 3 | Collective thrust | N | + +Zero rates and hover thrust hold the current attitude: + +```python +import numpy as np +from crazyflow.sim import Sim, Dynamics +from crazyflow.control import Control + +sim = Sim(control=Control.body_rate, dynamics=Dynamics.first_principles) +sim.reset() + +mass = float(sim.data.params.mass[0]) +cmd = np.zeros((1, 1, 4), dtype=np.float32) +cmd[0, 0, 3] = mass * 9.81 + +sim.body_rate_control(cmd) +sim.step(sim.freq // sim.control_freq) +``` + ## Force-torque control Direct force and torque input. Requires `Dynamics.first_principles`. @@ -157,6 +196,7 @@ Each control mode has its own update rate. The dynamics tick (`freq`) is always |---|---|---| | `state` | `state_freq` | 100 Hz | | `attitude` | `attitude_freq` | 500 Hz | +| `body_rate` | `body_rate_freq` | 500 Hz | | `force_torque` | `force_torque_freq` | 500 Hz | | `rotor_vel` | — | every dynamics step | @@ -167,7 +207,7 @@ The simulator applies a new command only when the control tick fires. Between ti The control modes above are how the simulator drives the onboard controllers. Those controllers also live in `crazyflow.control` as a self-contained library of pure functions, usable on their own for control design, learning-based policies, or as a reference implementation, independent of `Sim`. The following guides cover that standalone API: - [Controllers](controllers.md): the controller interface and the Mellinger pipeline -- [Mellinger controller](mellinger.md): the three stages, their inputs and outputs +- [Mellinger controller](mellinger.md): the three stages and the body rate variant, their inputs and outputs - [Parametrization](parametrize.md): binding a controller to a drone configuration - [Integral errors](integral-errors.md): carrying controller state across calls - [Batching](batching.md): evaluating many drones at once diff --git a/docs/user-guide/control/integral-errors.md b/docs/user-guide/control/integral-errors.md index 2f6223e0..6280cdde 100644 --- a/docs/user-guide/control/integral-errors.md +++ b/docs/user-guide/control/integral-errors.md @@ -54,7 +54,7 @@ for _ in range(10): ## Both stages have integral errors -`state2attitude` tracks position error via `pos_err_i`. `attitude2force_torque` tracks angular velocity error via `r_int_error`. Manage them independently: +`state2attitude` tracks position error via `pos_err_i`. `attitude2force_torque` and `body_rate2force_torque` track angular velocity error via `r_int_error`. Manage them independently: ```python import numpy as np diff --git a/docs/user-guide/control/mellinger.md b/docs/user-guide/control/mellinger.md index 07136e5b..c31058ad 100644 --- a/docs/user-guide/control/mellinger.md +++ b/docs/user-guide/control/mellinger.md @@ -1,6 +1,6 @@ # Mellinger controller -The Mellinger controller converts a full-state setpoint into individual motor speeds through three chained pure functions. The implementation closely follows the Crazyflie firmware to minimise sim-to-real gap. +The Mellinger controller converts a full-state setpoint into individual motor speeds through three chained pure functions. The implementation closely follows the Crazyflie firmware to minimise sim-to-real gap. A fourth function, `body_rate2force_torque`, replaces the second stage for body rate setpoints. ## State representation @@ -91,6 +91,47 @@ force.shape # (1,) torque.shape # (3,) ``` +## Body rates to force/torque {#body-rate-to-force-torque} + +`body_rate2force_torque` replaces stage 2 when the command is a body rate setpoint. The firmware has no dedicated body rate mode: a rate setpoint enters the angular velocity error and its derivative, while the attitude terms level the drone at its current yaw. The function reproduces this behaviour with the same gains as `attitude2force_torque`. To track body rates without the levelling terms, set `kR` and `ki_m` to zero. + +**Inputs:** + +| Argument | Shape | Description | +|---|---|---| +| `quat` | `(..., 4)` | Current attitude, xyzw | +| `ang_vel` | `(..., 3)` | Current angular velocity in body frame [rad/s] | +| `cmd` | `(..., 4)` | Body rate command: `[roll_rate, pitch_rate, yaw_rate, thrust_N]` | +| `prev_ang_vel` | `(..., 3)` or `None` | Angular velocity from the previous call. `None` initialises to zero | +| `prev_cmd` | `(..., 4)` or `None` | Command from the previous call, used for the setpoint derivative. `None` assumes a constant setpoint | +| `r_int_error` | `(..., 3)` or `None` | Angular velocity integral error from the previous call. `None` initialises to zero | +| `ctrl_freq` | `int` | Control frequency in Hz (default 500) | + +**Outputs:** + +| Return | Shape | Description | +|---|---|---| +| `force` | `(..., 1)` | Collective thrust [N] | +| `torque` | `(..., 3)` | Body-frame torques [N·m] | +| `r_int_error` | `(..., 3)` | Angular velocity integral error. Pass back as `r_int_error` on the next call | + +```python +import numpy as np +from crazyflow.control import load_params +from crazyflow.control.mellinger import body_rate2force_torque + +params = load_params(body_rate2force_torque, "cf2x_L250") +params["kR"], params["ki_m"] = np.zeros(3), np.zeros(3) # pure body rate tracking + +quat = np.array([0.0, 0.0, 0.0, 1.0]) +ang_vel = np.zeros(3) +cmd = np.array([0.5, 0.0, 0.0, 0.3]) # 0.5 rad/s roll rate, 0.3 N thrust + +force, torque, r_int_err = body_rate2force_torque(quat, ang_vel, cmd, **params) +force.shape # (1,) +torque.shape # (3,) +``` + ## Stage 3: Force/torque to rotor velocities {#force-torque-to-rotor-velocities} `force_torque2rotor_vel` converts collective thrust and body-frame torques into individual motor speeds, accounting for the motor mixing matrix. diff --git a/docs/user-guide/dynamics/index.md b/docs/user-guide/dynamics/index.md index f142965a..5f98feff 100644 --- a/docs/user-guide/dynamics/index.md +++ b/docs/user-guide/dynamics/index.md @@ -27,7 +27,7 @@ The first-principles dynamics derives forces and torques analytically from motor from crazyflow.sim import Sim, Dynamics from crazyflow.control import Control -# Force-torque and rotor_vel control modes require first_principles +# Body rate, force-torque and rotor_vel control modes require first_principles sim = Sim(dynamics=Dynamics.first_principles, control=Control.rotor_vel) sim.reset() ``` @@ -69,15 +69,15 @@ The `so_rpy_rotor_drag` variant includes translational drag, which captures the ## Control mode compatibility -| Dynamics | `Control.state` | `Control.attitude` | `Control.force_torque` | `Control.rotor_vel` | -|---|---|---|---|---| -| `first_principles` | ✓ | ✓ | ✓ | ✓ | -| `so_rpy` | ✓ | ✓ | ✗ | ✗ | -| `so_rpy_rotor` | ✓ | ✓ | ✗ | ✗ | -| `so_rpy_rotor_drag` | ✓ | ✓ | ✗ | ✗ | +| Dynamics | `Control.state` | `Control.attitude` | `Control.body_rate` | `Control.force_torque` | `Control.rotor_vel` | +|---|---|---|---|---|---| +| `first_principles` | ✓ | ✓ | ✓ | ✓ | ✓ | +| `so_rpy` | ✓ | ✓ | ✗ | ✗ | ✗ | +| `so_rpy_rotor` | ✓ | ✓ | ✗ | ✗ | ✗ | +| `so_rpy_rotor_drag` | ✓ | ✓ | ✗ | ✗ | ✗ | !!! warning - Using `Control.force_torque` or `Control.rotor_vel` with a fitted dynamics raises `ConfigError` at construction time. + Using `Control.body_rate`, `Control.force_torque` or `Control.rotor_vel` with a fitted dynamics raises `ConfigError` at construction time. ## Using the dynamics standalone diff --git a/docs/user-guide/functional-api.md b/docs/user-guide/functional-api.md index a491649e..de7a88fb 100644 --- a/docs/user-guide/functional-api.md +++ b/docs/user-guide/functional-api.md @@ -58,7 +58,7 @@ From this point, `data` is a plain JAX pytree and `step` and `reset` are compile ## Purely functional controller functions -`crazyflow.sim.functional` mirrors all four `Sim` control methods as pure functions: +`crazyflow.sim.functional` mirrors all five `Sim` control methods as pure functions: ```python import crazyflow.sim.functional as F @@ -68,6 +68,7 @@ import crazyflow.sim.functional as F |---|---| | `F.state_control(data, controls)` | Stage a state command | | `F.attitude_control(data, controls)` | Stage an attitude command | +| `F.body_rate_control(data, controls)` | Stage a body rate command | | `F.force_torque_control(data, controls)` | Stage a force/torque command | | `F.rotor_vel_control(data, controls)` | Stage rotor velocity commands | | `F.controllable(data)` | Boolean mask — which worlds may update their controller this step | diff --git a/docs/user-guide/index.md b/docs/user-guide/index.md index 99fbe308..a9ce0692 100644 --- a/docs/user-guide/index.md +++ b/docs/user-guide/index.md @@ -6,7 +6,7 @@ In-depth documentation for every part of the simulator. - [Object-Oriented API](oo-api.md) — `Sim` class, control methods, rendering, and reset - [Functional API](functional-api.md) — purely functional interface for JAX transformations - [Dynamics](dynamics/index.md) — first-principles vs. fitted dynamics, when to use each -- [Control Modes](control/index.md) — state, attitude, force/torque, and rotor velocity control +- [Control Modes](control/index.md) — state, attitude, body rate, force/torque, and rotor velocity control - [Pipelines](pipelines.md) — composable step and reset pipelines, randomization, and disturbances - [The world axis](world-axis.md) — which arrays are batched over worlds, and what resets and sharding do with them - [Visualization](visualization.md) — rendering modes, cameras, raycasting, and materials diff --git a/docs/user-guide/oo-api.md b/docs/user-guide/oo-api.md index a22574d2..277ae22c 100644 --- a/docs/user-guide/oo-api.md +++ b/docs/user-guide/oo-api.md @@ -77,7 +77,7 @@ import numpy as np from crazyflow.sim import Sim, Dynamics from crazyflow.control import Control -sim = Sim(n_worlds=1, n_drones=1, control=Control.attitude, dynamics=Dynamics.so_rpy) +sim = Sim(n_worlds=1, n_drones=1, control=Control.attitude) sim.reset() # [roll, pitch, yaw, collective_thrust_N] @@ -88,6 +88,26 @@ sim.attitude_control(cmd) sim.step(sim.freq // sim.control_freq) ``` +### Body rate control + +Commands body-frame angular rates (rad/s) and a collective thrust (N). The Mellinger controller tracks the rates and, as in the firmware, levels the drone with its attitude terms. Set `kR` and `ki_m` to zero for pure rate tracking, see [Control Modes](control/index.md#body-rate-control). Requires `Dynamics.first_principles`. + +```python +import numpy as np +from crazyflow.sim import Sim, Dynamics +from crazyflow.control import Control + +sim = Sim(n_worlds=1, n_drones=1, control=Control.body_rate) +sim.reset() + +# [body_rate_x, body_rate_y, body_rate_z, collective_thrust_N] +cmd = np.zeros((1, 1, 4), dtype=np.float32) +cmd[0, 0, 3] = float(sim.data.params.mass[0]) * 9.81 # hover thrust + +sim.body_rate_control(cmd) +sim.step(sim.freq // sim.control_freq) +``` + ### Force-torque control Direct force and torque input, useful for testing dynamics or custom controllers. Requires `Dynamics.first_principles`. diff --git a/tests/integration/test_interfaces.py b/tests/integration/test_interfaces.py index 34b1036a..17b5c26c 100644 --- a/tests/integration/test_interfaces.py +++ b/tests/integration/test_interfaces.py @@ -1,4 +1,5 @@ import jax +import jax.numpy as jnp import numpy as np import pytest from scipy.spatial.transform import Rotation as R @@ -55,6 +56,40 @@ def test_attitude_interface(dynamics: Dynamics): assert distance < 0.05, f"Failed to maintain hover with {dynamics} ({dpos})" +@pytest.mark.integration +def test_body_rate_interface(): + sim = Sim(dynamics=Dynamics.first_principles, control=Control.body_rate) + # Disable the attitude terms of the firmware controller to track body rates directly + body_rate = sim.data.controls.body_rate + params = body_rate.params | {"kR": jnp.zeros(3), "ki_m": jnp.zeros(3)} + controls = sim.data.controls.replace(body_rate=body_rate.replace(params=params)) + sim.data = sim.data.replace(controls=controls) + target_pos = np.array([0.0, 0.0, 1.0]) + jit_state2attitude = jax.jit(parametrize(state2attitude, drone=sim.drone)) + kp_att = 8.0 # Proportional gain from attitude error to body rates + + pos_err_i = np.zeros((1, 1, 3)) + cmd = np.zeros((1, 1, 13)) + body_rate_cmd = np.zeros((1, 1, 4)) + steps = int(3 * sim.control_freq) + + for i in range(steps): + cmd[..., :3] = target_pos * i / steps # Linearly interpolate target position + pos, vel, quat = sim.data.states.pos, sim.data.states.vel, sim.data.states.quat + rpyt, pos_err_i = jit_state2attitude(pos, quat, vel, cmd, pos_err_i, ctrl_freq=500) + rpyt = np.asarray(rpyt)[0, 0] + rot_err = R.from_quat(np.asarray(quat[0, 0])).inv() * R.from_euler("xyz", rpyt[:3]) + body_rate_cmd[0, 0, :3] = kp_att * rot_err.as_rotvec() + body_rate_cmd[0, 0, 3] = rpyt[3] + sim.body_rate_control(body_rate_cmd) + sim.step(sim.freq // sim.control_freq) + + # Check if drone maintained hover position + dpos = sim.data.states.pos[0, 0] - target_pos + distance = np.linalg.norm(dpos) + assert distance < 0.05, f"Failed to maintain hover with body rate control ({dpos})" + + @pytest.mark.integration def test_rotor_vel_interface(): sim = Sim(dynamics=Dynamics.first_principles, control=Control.rotor_vel) diff --git a/tests/unit/control/test_core.py b/tests/unit/control/test_core.py index 1fd698cf..4b2f2dbd 100644 --- a/tests/unit/control/test_core.py +++ b/tests/unit/control/test_core.py @@ -9,12 +9,18 @@ from crazyflow.control import load_params, parametrize from crazyflow.control.mellinger import ( attitude2force_torque, + body_rate2force_torque, force_torque2rotor_vel, state2attitude, ) from crazyflow.drones import available_drones -_MELLINGER_FNS = [state2attitude, attitude2force_torque, force_torque2rotor_vel] +_MELLINGER_FNS = [ + state2attitude, + attitude2force_torque, + body_rate2force_torque, + force_torque2rotor_vel, +] @pytest.mark.unit diff --git a/tests/unit/control/test_mellinger.py b/tests/unit/control/test_mellinger.py index 60298948..b4a518db 100644 --- a/tests/unit/control/test_mellinger.py +++ b/tests/unit/control/test_mellinger.py @@ -4,10 +4,12 @@ import numpy as np import pytest +from scipy.spatial.transform import Rotation as R from crazyflow.control import load_params, parametrize from crazyflow.control.mellinger import ( attitude2force_torque, + body_rate2force_torque, force_torque2rotor_vel, state2attitude, ) @@ -59,6 +61,27 @@ def test_attitude2force_torque(drone: str) -> None: assert r_int_error.shape == (5, 4, 3) +@pytest.mark.unit +@pytest.mark.parametrize("drone", available_drones) +def test_body_rate2force_torque(drone: str) -> None: + controller = parametrize(body_rate2force_torque, drone) + # Single input + _, quat, _, ang_vel = create_rnd_states() + cmd = np.array([0.1, 0.1, 0.1, 1.0]) # roll rate, pitch rate, yaw rate, thrust command + force_des, torque_des, r_int_error = controller(quat, ang_vel, cmd) + assert force_des.shape == (1,) + assert torque_des.shape == (3,) + assert r_int_error.shape == (3,) + # Batch input + _, quat, _, ang_vel = create_rnd_states((5, 4)) + cmd = np.random.randn(5, 4, 4) + cmd[..., 3] = np.abs(cmd[..., 3]) # Ensure positive thrust + force_des, torque_des, r_int_error = controller(quat, ang_vel, cmd) + assert force_des.shape == (5, 4, 1) + assert torque_des.shape == (5, 4, 3) + assert r_int_error.shape == (5, 4, 3) + + @pytest.mark.unit @pytest.mark.parametrize("drone", available_drones) def test_force_torque2rotor_vel(drone: str) -> None: @@ -147,6 +170,88 @@ def test_attitude2force_torque_zero_thrust(drone: str): assert np.allclose(torque_des, 0.0, atol=1e-6) +@pytest.mark.unit +@pytest.mark.parametrize("drone", available_drones) +def test_body_rate2force_torque_at_setpoint(drone: str) -> None: + # Level drone with measured rates equal to the commanded rates → zero corrective torque. + controller = parametrize(body_rate2force_torque, drone) + quat = R.from_euler("xyz", [0.0, 0.0, 0.7]).as_quat() # Any yaw is level + ang_vel = np.array([0.3, -0.2, 0.1]) + cmd = np.array([0.3, -0.2, 0.1, 0.5]) + force_des, torque_des, _ = controller(quat, ang_vel, cmd, prev_ang_vel=ang_vel) + assert np.allclose(torque_des, 0.0, atol=1e-6), ( + f"Torque at setpoint should be ~0, got {torque_des}" + ) + assert force_des[0] > 0.0, "Force must be positive for positive thrust command" + + +@pytest.mark.unit +@pytest.mark.parametrize("drone", available_drones) +def test_body_rate2force_torque_zero_thrust(drone: str): + # Zero thrust command → firmware zeros torque; outputs are all zero. + controller = parametrize(body_rate2force_torque, drone) + quat = np.array([0.0, 0.0, 0.0, 1.0]) + ang_vel = np.zeros(3) + cmd = np.array([0.1, 0.1, 0.1, 0.0]) # non-zero rates but zero thrust + force_des, torque_des, _ = controller(quat, ang_vel, cmd) + assert np.allclose(force_des, 0.0, atol=1e-6) + assert np.allclose(torque_des, 0.0, atol=1e-6) + + +@pytest.mark.unit +@pytest.mark.parametrize("drone", available_drones) +def test_body_rate2force_torque_sign(drone: str): + # A positive rate error about one axis must produce a positive torque about that axis only. + controller = parametrize(body_rate2force_torque, drone) + quat = np.array([0.0, 0.0, 0.0, 1.0]) + ang_vel = np.zeros(3) + for axis in range(3): + cmd = np.array([0.0, 0.0, 0.0, 0.5]) + cmd[axis] = 1.0 + _, torque_des, _ = controller(quat, ang_vel, cmd, prev_ang_vel=ang_vel, prev_cmd=cmd) + assert torque_des[axis] > 0.0, f"Torque about axis {axis} must be positive: {torque_des}" + others = np.delete(torque_des, axis) + assert np.allclose(others, 0.0, atol=1e-6), f"Cross-axis torque for axis {axis}: {others}" + + +@pytest.mark.unit +@pytest.mark.parametrize("drone", available_drones) +def test_body_rate2force_torque_matches_attitude(drone: str): + # A zero body rate command is equivalent to commanding a level attitude at the current yaw. + att_controller = parametrize(attitude2force_torque, drone) + rate_controller = parametrize(body_rate2force_torque, drone) + quat = R.from_euler("xyz", [0.2, -0.1, 0.7]).as_quat() + ang_vel = np.array([0.1, -0.2, 0.05]) + prev_ang_vel = np.array([0.05, -0.1, 0.0]) + att_cmd = np.array([0.0, 0.0, 0.7, 0.5]) + rate_cmd = np.array([0.0, 0.0, 0.0, 0.5]) + force_att, torque_att, err_att = att_controller( + quat, ang_vel, att_cmd, prev_ang_vel=prev_ang_vel + ) + force_rate, torque_rate, err_rate = rate_controller( + quat, ang_vel, rate_cmd, prev_ang_vel=prev_ang_vel + ) + assert np.allclose(force_att, force_rate, atol=1e-6) + assert np.allclose(torque_att, torque_rate, atol=1e-6) + assert np.allclose(err_att, err_rate, atol=1e-6) + + +@pytest.mark.unit +@pytest.mark.parametrize("drone", available_drones) +def test_body_rate2force_torque_leveling(drone: str): + # The firmware levels a tilted drone even at the rate setpoint. Zero attitude gains disable it. + controller = parametrize(body_rate2force_torque, drone) + params = load_params(body_rate2force_torque, drone) + quat = R.from_euler("xyz", [0.2, 0.0, 0.0]).as_quat() # Rolled by 0.2 rad + ang_vel = np.zeros(3) + cmd = np.array([0.0, 0.0, 0.0, 0.5]) + _, torque_des, _ = controller(quat, ang_vel, cmd) + assert torque_des[0] < 0.0, f"Leveling torque must oppose the roll, got {torque_des}" + params["kR"], params["ki_m"] = np.zeros(3), np.zeros(3) + _, torque_des, _ = body_rate2force_torque(quat, ang_vel, cmd, **params) + assert np.allclose(torque_des, 0.0, atol=1e-6), f"Torque with zero attitude gains {torque_des}" + + # Batch consistency (batch result == sequential result) @@ -182,6 +287,33 @@ def test_attitude2force_torque_batch_consistency(drone: str): assert np.allclose(err_batch[i, j], err_s, atol=1e-5) +@pytest.mark.unit +@pytest.mark.parametrize("drone", available_drones) +def test_body_rate2force_torque_batch_consistency(drone: str): + controller = parametrize(body_rate2force_torque, drone) + batch = (3, 2) + _, quat, _, ang_vel = create_rnd_states(batch) + _, _, _, prev_ang_vel = create_rnd_states(batch) + cmd = np.random.randn(*batch, 4) + cmd[..., 3] = np.abs(cmd[..., 3]) + prev_cmd = np.random.randn(*batch, 4) + force_batch, torque_batch, err_batch = controller( + quat, ang_vel, cmd, prev_ang_vel=prev_ang_vel, prev_cmd=prev_cmd + ) + for i in range(batch[0]): + for j in range(batch[1]): + force_s, torque_s, err_s = controller( + quat[i, j], + ang_vel[i, j], + cmd[i, j], + prev_ang_vel=prev_ang_vel[i, j], + prev_cmd=prev_cmd[i, j], + ) + assert np.allclose(force_batch[i, j], force_s, atol=1e-5) + assert np.allclose(torque_batch[i, j], torque_s, atol=1e-5) + assert np.allclose(err_batch[i, j], err_s, atol=1e-5) + + @pytest.mark.unit @pytest.mark.parametrize("drone", available_drones) def test_force_torque2rotor_vel_batch_consistency(drone: str): diff --git a/tests/unit/test_sim.py b/tests/unit/test_sim.py index 3d1d4157..6dca9d98 100644 --- a/tests/unit/test_sim.py +++ b/tests/unit/test_sim.py @@ -58,7 +58,7 @@ def test_sim_init(dynamics: Dynamics, device: str, control: Control, n_worlds: i n_drones = 1 if dynamics != Dynamics.first_principles: - if control in (Control.force_torque, Control.rotor_vel): + if control in (Control.body_rate, Control.force_torque, Control.rotor_vel): with pytest.raises(ConfigError): Sim(n_worlds=n_worlds, dynamics=dynamics, device=device, control=control) return @@ -89,9 +89,16 @@ def test_sim_init(dynamics: Dynamics, device: str, control: Control, n_worlds: i array_meta_assert(sim.data.controls.attitude.cmd, (n_worlds, n_drones, 4), device) else: assert sim.data.controls.attitude is None + # Test body rate buffer shapes + if control == Control.body_rate: + assert isinstance(sim.data.controls.body_rate, ControlData) + array_meta_assert(sim.data.controls.body_rate.staged_cmd, (n_worlds, n_drones, 4), device) + array_meta_assert(sim.data.controls.body_rate.cmd, (n_worlds, n_drones, 4), device) + else: + assert sim.data.controls.body_rate is None # Test force torque buffer shapes - if control in (Control.state, Control.attitude, Control.force_torque): + if control in (Control.state, Control.attitude, Control.body_rate, Control.force_torque): ft_ctrl = sim.data.controls.force_torque assert isinstance(ft_ctrl, ControlData) array_meta_assert(ft_ctrl.cmd, (n_worlds, n_drones, 4), device) @@ -192,7 +199,7 @@ def test_reset_masked(device: str, dynamics: Dynamics): @pytest.mark.parametrize("control", Control) def test_sim_step(n_worlds: int, n_drones: int, dynamics: Dynamics, control: Control, device: str): if dynamics != Dynamics.first_principles: - if control in (Control.force_torque, Control.rotor_vel): + if control in (Control.body_rate, Control.force_torque, Control.rotor_vel): pytest.skip(f"{control} is not supported with non-first-principles dynamics") sim = Sim( From eb34b70e774e2a1429820cefb603e9fdb24c3334 Mon Sep 17 00:00:00 2001 From: ratheron Date: Thu, 10 Sep 2026 12:40:23 +0200 Subject: [PATCH 2/6] Add example. Closes #52. --- crazyflow/control/mellinger/control.py | 12 ++-- examples/control/attitude.py | 96 ++++++++++++-------------- examples/control/body_rate.py | 92 ++++++++++++++++++++++++ tests/unit/test_sim.py | 1 + 4 files changed, 142 insertions(+), 59 deletions(-) create mode 100644 examples/control/body_rate.py diff --git a/crazyflow/control/mellinger/control.py b/crazyflow/control/mellinger/control.py index 0d3c4704..384b6dc8 100644 --- a/crazyflow/control/mellinger/control.py +++ b/crazyflow/control/mellinger/control.py @@ -590,17 +590,17 @@ def create( n_worlds: int, n_drones: int, freq: int, drone: str, device: Device ) -> MellingerBodyRateData: """Create a default set of body rate data for the simulation.""" - cmd = jnp.zeros((n_worlds, n_drones, 4), device=device) - steps = -jnp.ones((n_worlds, 1), dtype=jnp.int32, device=device) zeros_3d = jnp.zeros((n_worlds, n_drones, 3), device=device) + zeros_4d = jnp.zeros((n_worlds, n_drones, 4), device=device) + steps = -jnp.ones((n_worlds, 1), dtype=jnp.int32, device=device) params = load_params(body_rate2force_torque, drone, xp=jnp, device=device) return MellingerBodyRateData( - cmd=cmd, - staged_cmd=cmd, + cmd=zeros_4d.copy(), + staged_cmd=zeros_4d.copy(), steps=steps, freq=freq, - r_int_error=zeros_3d, - last_ang_vel=zeros_3d, + r_int_error=zeros_3d.copy(), + last_ang_vel=zeros_3d.copy(), params=params, ) diff --git a/examples/control/attitude.py b/examples/control/attitude.py index 66c7fc37..7c4053e2 100644 --- a/examples/control/attitude.py +++ b/examples/control/attitude.py @@ -1,58 +1,41 @@ -import os +from functools import partial +from typing import Callable import numpy as np -os.environ["SCIPY_ARRAY_API"] = "1" - -from scipy.spatial.transform import Rotation as R - -from crazyflow.control import Control +from crazyflow.control import Control, parametrize +from crazyflow.control.mellinger import state2attitude from crazyflow.sim import Sim -kp = np.array([0.4, 0.4, 1.25]) -ki = np.array([0.05, 0.05, 0.05]) -kd = np.array([0.2, 0.2, 0.4]) -g = 9.81 - def control( - t: float, obs: dict[str, np.ndarray], pos_start: np.ndarray, drone_mass: float -) -> np.ndarray: - des_pos = np.zeros(3) - des_pos[..., :2] = pos_start[:2] + np.array([np.cos(t) - 1, np.sin(t)]) - des_pos[..., 2] = 0.2 * t - des_vel = np.zeros_like(des_pos) - des_yaw = t - - # Calculate the deviations from the desired trajectory - pos_error = des_pos - np.array(obs["pos"]) - vel_error = des_vel - np.array(obs["vel"]) - - # Compute target thrust - target_thrust = np.zeros(3) - target_thrust += kp * pos_error - target_thrust += kd * vel_error - target_thrust[2] += drone_mass * g - - # Update z_axis to the current orientation of the drone - z_axis = R.from_quat(obs["quat"]).as_matrix()[:, 2] - - # update current thrust - thrust_desired = target_thrust.dot(z_axis) - - # update z_axis_desired - z_axis_desired = target_thrust / np.linalg.norm(target_thrust) - x_c_des = np.array([np.cos(des_yaw), np.sin(des_yaw), 0.0]) - y_axis_desired = np.cross(z_axis_desired, x_c_des) - y_axis_desired /= np.linalg.norm(y_axis_desired) - x_axis_desired = np.cross(y_axis_desired, z_axis_desired) - - R_desired = np.vstack([x_axis_desired, y_axis_desired, z_axis_desired]).T - euler_desired = R.from_matrix(R_desired).as_euler("xyz", degrees=False) - - action = np.concatenate([euler_desired, [thrust_desired]], dtype=np.float32) - - return action + t: float, + obs: dict[str, np.ndarray], + pos_start: np.ndarray, + position_ctrl: Callable, + pos_err_i: np.ndarray, +) -> tuple[np.ndarray, np.ndarray]: + """Compute the attitude command to track a circle with a slow climb. + + Args: + t: Time since the start of the trajectory in s. + obs: Drone position, orientation, and velocity. + pos_start: Start position of the drone. + position_ctrl: Controller that maps the state and a full state command to an attitude + command. Any controller that outputs [roll, pitch, yaw, thrust] can be used here. + pos_err_i: Integral error of the position controller from the previous call. + + Returns: + The attitude command [roll, pitch, yaw, thrust] in rad and N, and the updated integral + error. + """ + # Full state command with velocity and acceleration feedforward + cmd = np.zeros(13) + cmd[:3] = pos_start + np.array([np.cos(t) - 1, np.sin(t), 0.2 * t]) + cmd[3:6] = np.array([-np.sin(t), np.cos(t), 0.2]) + cmd[6:9] = np.array([-np.cos(t), -np.sin(t), 0.0]) + cmd[9] = t # Yaw + return position_ctrl(obs["pos"], obs["quat"], obs["vel"], cmd, pos_err_i) def main(): @@ -61,15 +44,22 @@ def main(): duration = 6.5 fps = 60 + # We use the Mellinger position controller to generate attitude commands. This could be any + # controller that outputs [roll, pitch, yaw, thrust], e.g. a learned policy. + position_ctrl = partial(parametrize(state2attitude, sim.drone), ctrl_freq=sim.control_freq) + pos_err_i = np.zeros(3) cmd = np.zeros((sim.n_worlds, sim.n_drones, 4)) # [roll, pitch, yaw, thrust] - pos_start = sim.data.states.pos + pos_start = np.asarray(sim.data.states.pos[0, 0]) for i in range(int(duration * sim.control_freq)): + # Convert the states to numpy so that the controller runs in numpy instead of eager JAX obs = { - "pos": sim.data.states.pos[0, 0], - "vel": sim.data.states.vel[0, 0], - "quat": sim.data.states.quat[0, 0], + "pos": np.asarray(sim.data.states.pos[0, 0]), + "quat": np.asarray(sim.data.states.quat[0, 0]), + "vel": np.asarray(sim.data.states.vel[0, 0]), } - cmd[0, 0, :] = control(i / sim.control_freq, obs, pos_start[0, 0], sim.data.params.mass[0]) + cmd[0, 0, :], pos_err_i = control( + i / sim.control_freq, obs, pos_start, position_ctrl, pos_err_i + ) sim.attitude_control(cmd) sim.step(sim.freq // sim.control_freq) if ((i * fps) % sim.control_freq) < fps: diff --git a/examples/control/body_rate.py b/examples/control/body_rate.py new file mode 100644 index 00000000..b92b1d59 --- /dev/null +++ b/examples/control/body_rate.py @@ -0,0 +1,92 @@ +import os +from functools import partial +from typing import Callable + +import jax.numpy as jnp +import numpy as np + +os.environ["SCIPY_ARRAY_API"] = "1" + +from scipy.spatial.transform import Rotation as R + +from crazyflow.control import Control, parametrize +from crazyflow.control.mellinger import state2attitude +from crazyflow.sim import Sim + +kp_att = 8.0 # Proportional gain from attitude error to body rates + + +def control( + t: float, + obs: dict[str, np.ndarray], + pos_start: np.ndarray, + position_ctrl: Callable, + pos_err_i: np.ndarray, +) -> tuple[np.ndarray, np.ndarray]: + """Compute the body rate command to track a circle with a slow climb. + + The attitude command of the position controller is converted into body rates with a + proportional attitude loop. + + Args: + t: Time since the start of the trajectory in s. + obs: Drone position, orientation, and velocity. + pos_start: Start position of the drone. + position_ctrl: Controller that maps the state and a full state command to an attitude + command. Any controller that outputs [roll, pitch, yaw, thrust] can be used here. + pos_err_i: Integral error of the position controller from the previous call. + + Returns: + The body rate command [roll_rate, pitch_rate, yaw_rate, thrust] in rad/s and N, and the + updated integral error. + """ + # Full state command with velocity and acceleration feedforward + cmd = np.zeros(13) + cmd[:3] = pos_start + np.array([np.cos(t) - 1, np.sin(t), 0.2 * t]) + cmd[3:6] = np.array([-np.sin(t), np.cos(t), 0.2]) + cmd[6:9] = np.array([-np.cos(t), -np.sin(t), 0.0]) + cmd[9] = t # Yaw + rpyt, pos_err_i = position_ctrl(obs["pos"], obs["quat"], obs["vel"], cmd, pos_err_i) + rot_err = (R.from_quat(obs["quat"]).inv() * R.from_euler("xyz", rpyt[:3])).as_rotvec() + return np.concatenate([kp_att * rot_err, rpyt[3:]]), pos_err_i + + +def main(): + sim = Sim(control=Control.body_rate, body_rate_freq=250) + # The firmware has no dedicated body rate mode. Its attitude terms level the drone at the + # current yaw and would counteract the commanded rates. Disable them to track body rates. + body_rate = sim.data.controls.body_rate + params = body_rate.params | {"kR": jnp.zeros(3), "ki_m": jnp.zeros(3)} + sim.data = sim.data.replace( + controls=sim.data.controls.replace(body_rate=body_rate.replace(params=params)) + ) + sim.build_default_data() + sim.reset() + duration = 6.5 + fps = 60 + + # We use the Mellinger position controller to generate attitude commands. This could be any + # controller that outputs [roll, pitch, yaw, thrust], e.g. a learned policy. + position_ctrl = partial(parametrize(state2attitude, sim.drone), ctrl_freq=sim.control_freq) + pos_err_i = np.zeros(3) + cmd = np.zeros((sim.n_worlds, sim.n_drones, 4)) # [roll_rate, pitch_rate, yaw_rate, thrust] + pos_start = np.asarray(sim.data.states.pos[0, 0]) + for i in range(int(duration * sim.control_freq)): + # Convert the states to numpy so that the controller runs in numpy instead of eager JAX + obs = { + "pos": np.asarray(sim.data.states.pos[0, 0]), + "quat": np.asarray(sim.data.states.quat[0, 0]), + "vel": np.asarray(sim.data.states.vel[0, 0]), + } + cmd[0, 0, :], pos_err_i = control( + i / sim.control_freq, obs, pos_start, position_ctrl, pos_err_i + ) + sim.body_rate_control(cmd) + sim.step(sim.freq // sim.control_freq) + if ((i * fps) % sim.control_freq) < fps: + sim.render() + sim.close() + + +if __name__ == "__main__": + main() diff --git a/tests/unit/test_sim.py b/tests/unit/test_sim.py index 0403d97f..6d5cabda 100644 --- a/tests/unit/test_sim.py +++ b/tests/unit/test_sim.py @@ -111,6 +111,7 @@ def test_sim_init(dynamics: Dynamics, device: str, control: Control, n_worlds: i def test_sim_data_buffers_are_distinct(dynamics: Dynamics, control: Control, device: str): """Every leaf of SimData must own its buffer, or XLA refuses to donate the pytree.""" if dynamics != Dynamics.first_principles and control in ( + Control.body_rate, Control.force_torque, Control.rotor_vel, ): From 1c27e399c788767f8a08bb663695bebdb34328ea Mon Sep 17 00:00:00 2001 From: ratheron Date: Thu, 10 Sep 2026 15:30:27 +0200 Subject: [PATCH 3/6] Fix state interface --- README.md | 2 +- benchmark/performance.py | 2 +- benchmark/splat.py | 2 +- crazyflow/control/core.py | 18 +++-- crazyflow/control/mellinger/control.py | 76 +++++++++++++++------- crazyflow/sim/functional.py | 2 +- docs/examples/index.md | 2 +- docs/get-started/quick-start.md | 22 +++---- docs/index.md | 4 +- docs/user-guide/control/batching.md | 4 +- docs/user-guide/control/controllers.md | 2 +- docs/user-guide/control/index.md | 28 ++++---- docs/user-guide/control/integral-errors.md | 6 +- docs/user-guide/control/jit.md | 6 +- docs/user-guide/control/mellinger.md | 12 ++-- docs/user-guide/control/parametrize.md | 4 +- docs/user-guide/oo-api.md | 12 ++-- docs/user-guide/sim-overview.md | 2 +- examples/contacts/crash.py | 4 +- examples/control/attitude.py | 4 +- examples/control/body_rate.py | 9 ++- examples/control/change_pos.py | 2 +- examples/control/dynamics.py | 2 +- examples/control/hover.py | 4 +- examples/control/spiral.py | 2 +- examples/plugins/action_delay.py | 2 +- examples/plugins/disturbance.py | 2 +- examples/plugins/estimation.py | 2 +- examples/plugins/randomize.py | 2 +- examples/rendering/cam_config.py | 2 +- examples/rendering/cameras.py | 2 +- examples/rendering/led_deck.py | 2 +- examples/rendering/splat_camera.py | 5 +- examples/rendering/splat_depth.py | 7 +- examples/rendering/splat_viewer.py | 2 +- tests/integration/test_disturbance.py | 2 +- tests/integration/test_interfaces.py | 37 +++++++++-- tests/unit/control/test_mellinger.py | 12 ++-- tests/unit/test_functional.py | 4 +- tests/unit/test_gradients.py | 2 +- tests/unit/test_sim.py | 28 ++++++-- 41 files changed, 212 insertions(+), 134 deletions(-) diff --git a/README.md b/README.md index 63362496..9ec63eea 100644 --- a/README.md +++ b/README.md @@ -20,7 +20,7 @@ from crazyflow.sim import Sim from crazyflow.control import Control sim = Sim(n_worlds=4096, n_drones=1, control=Control.state) -cmd = np.zeros((4096, 1, 13)) +cmd = np.zeros((4096, 1, 16)) cmd[..., 2] = 0.5 # hover at 0.5 m across all worlds for _ in range(100): diff --git a/benchmark/performance.py b/benchmark/performance.py index 1d7c49d0..4a0c31be 100644 --- a/benchmark/performance.py +++ b/benchmark/performance.py @@ -19,7 +19,7 @@ def profile_step(sim_config: config_dict.ConfigDict, n_steps: int, device: str): sim = Sim(**sim_config) device = jax.devices(device)[0] - ndim = 13 if sim.control == "state" else 4 + ndim = 16 if sim.control == "state" else 4 control_fn = sim.state_control if sim.control == "state" else sim.attitude_control cmd = np.zeros((sim.n_worlds, sim.n_drones, ndim)) # Ensure JIT compiled dynamics and control diff --git a/benchmark/splat.py b/benchmark/splat.py index 2e130b63..004cface 100644 --- a/benchmark/splat.py +++ b/benchmark/splat.py @@ -118,7 +118,7 @@ def benchmark( # Hold a constant target so the drone keeps moving and each frame renders a distinct # pose. A static scene would let XLA hoist the render out of the loop. - cmd = np.zeros((sim.n_worlds, sim.n_drones, 13), dtype=np.float32) + cmd = np.zeros((sim.n_worlds, sim.n_drones, 16), dtype=np.float32) cmd[..., 2] = 0.5 sim.reset() sim.state_control(jnp.asarray(cmd, device=sim.device)) diff --git a/crazyflow/control/core.py b/crazyflow/control/core.py index ccd50dd7..808a8239 100644 --- a/crazyflow/control/core.py +++ b/crazyflow/control/core.py @@ -41,7 +41,7 @@ def parametrize( ctrl = parametrize(state2attitude, "cf2x_L250") pos, quat = np.zeros(3), np.array([0.0, 0.0, 0.0, 1.0]) - vel, cmd = np.zeros(3), np.zeros(13) + vel, cmd = np.zeros(3), np.zeros(16) rpyt, int_pos_err = ctrl(pos, quat, vel, cmd) ``` @@ -85,14 +85,18 @@ class Control(StrEnum): """Control type of the simulated onboard controller.""" state = "state" - """State control takes [x, y, z, vx, vy, vz, ax, ay, az, yaw, roll_rate, pitch_rate, yaw_rate]. - + """State control takes [x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, wx, wy, wz]. + + The attitude setpoint qx, qy, qz, qw is an xyzw quaternion. The body rates wx, wy, wz are the + angular velocity in the body frame in rad/s. The position controller forwards them to the + attitude controller as its body rate setpoint. + Note: Recommended frequency is >=20 Hz. Warning: - Currently, we only use positions, velocities, and yaw. The rest of the state is ignored. - This is subject to change in the future. + Only the yaw of the attitude quaternion is used, as in the firmware. The fitted dynamics + (so_rpy family) take the attitude command directly and ignore the body rates. """ attitude = "attitude" """Attitude control takes [roll, pitch, yaw, collective thrust]. @@ -101,7 +105,9 @@ class Control(StrEnum): Recommended frequency is >=100 Hz. """ body_rate = "body_rate" - """Body rate control takes [roll_rate, pitch_rate, yaw_rate, collective thrust]. + """Body rate control takes [wx, wy, wz, collective thrust]. + + The body rates wx, wy, wz are the angular velocity in the body frame in rad/s. Note: Recommended frequency is >=200 Hz. diff --git a/crazyflow/control/mellinger/control.py b/crazyflow/control/mellinger/control.py index 384b6dc8..716d5be4 100644 --- a/crazyflow/control/mellinger/control.py +++ b/crazyflow/control/mellinger/control.py @@ -58,8 +58,12 @@ def state2attitude( pos: Drone position with shape (..., 3). quat: Drone orientation as xyzw quaternion with shape (..., 4). vel: Drone velocity with shape (..., 3). - cmd: Full state command in SI units and rad with shape (..., 13). The entries are - [x, y, z, vx, vy, vz, ax, ay, az, yaw, roll_rate, pitch_rate, yaw_rate]. + cmd: Full state command in SI units with shape (..., 16). The entries are + [x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, wx, wy, wz], where qx, qy, qz, qw is + the attitude setpoint as xyzw quaternion and wx, wy, wz are the body rates in rad/s. As + in the firmware, only the yaw of the attitude setpoint is used. The body rates are not + used by the position controller. They are the body rate setpoint of the attitude + controller. pos_err_i: Position integral error (..., 3) from the previous call. If None, it is initialised to zero. ctrl_freq: Control frequency in Hz @@ -83,9 +87,8 @@ def state2attitude( setpoint_pos = cmd[..., 0:3] setpoint_vel = cmd[..., 3:6] setpoint_acc = cmd[..., 6:9] - setpoint_yaw = cmd[..., 9] + setpoint_quat = cmd[..., 9:13] dt = 1 / ctrl_freq - # setpointRPY_rates = cmd[..., 10:13] # From firmware controller_mellinger pos_err = setpoint_pos - pos # l. 145 Position Error (ep) vel_err = setpoint_vel - vel # l. 148 Velocity Error (ev) @@ -98,9 +101,10 @@ def state2attitude( target_thrust = ( mass * (setpoint_acc - gravity_vec) + kp * pos_err + kd * vel_err + ki * int_pos_err ) - # l. 178 Rate-controlled YAW is moving YAW angle setpoint - # => only one case here, since the setpoint is always in absolute mode - desired_yaw = setpoint_yaw + # l. 166 ff Desired yaw from the quaternion setpoint. Only the yaw of the setpoint attitude is + # used, as in the firmware + qx, qy, qz, qw = (setpoint_quat[..., i] for i in range(4)) + desired_yaw = xp.atan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy**2 + qz**2)) # l. 189 Z-Axis [zB] rot = R.from_quat(quat).as_matrix() z_axis = rot[..., -1] # 3rd column or roation matrix is z axis @@ -489,13 +493,14 @@ def force_torque2rotor_vel( @dataclass class MellingerStateData: - cmd: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 13) + cmd: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 16) """Full state control command for the drone. - A command consists of [x, y, z, vx, vy, vz, ax, ay, az, yaw, roll_rate, pitch_rate, yaw_rate]. - We currently do not use the acceleration and angle rate components. This is subject to change. + A command consists of [x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, wx, wy, wz]. Only the + yaw of the attitude quaternion is used. The body rates wx, wy, wz are forwarded to the attitude + controller as its body rate setpoint. """ - staged_cmd: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 13) + staged_cmd: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 16) """Staging buffer to store the most recent command until the next controller tick.""" steps: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, 1) """Last simulation steps that the state control command was applied.""" @@ -512,12 +517,12 @@ def create( ) -> MellingerStateData: """Create a default set of state data for the simulation.""" zeros_3d = jnp.zeros((n_worlds, n_drones, 3), device=device) - zeros_13d = jnp.zeros((n_worlds, n_drones, 13), device=device) + cmd = jnp.zeros((n_worlds, n_drones, 16), device=device).at[..., 12].set(1.0) steps = -jnp.ones((n_worlds, 1), dtype=jnp.int32, device=device) params = load_params(state2attitude, drone, xp=jnp, device=device) return MellingerStateData( - cmd=zeros_13d.copy(), - staged_cmd=zeros_13d.copy(), + cmd=cmd, + staged_cmd=cmd.copy(), steps=steps, freq=freq, pos_err_i=zeros_3d.copy(), @@ -534,6 +539,13 @@ class MellingerAttitudeData: """ staged_cmd: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 4) """Staging buffer to store the most recent command until the next controller tick.""" + ang_vel_des: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 3) + """Body rate setpoint [wx, wy, wz] of the attitude controller. + + Zero for attitude control. State control forwards the body rates of the state command. + """ + staged_ang_vel_des: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 3) + """Staging buffer to store the most recent body rate setpoint until the next controller tick.""" steps: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, 1) """Last simulation steps that the attitude control command was applied.""" freq: int = field(pytree_node=False) @@ -557,6 +569,8 @@ def create( return MellingerAttitudeData( cmd=zeros_4d.copy(), staged_cmd=zeros_4d.copy(), + ang_vel_des=zeros_3d.copy(), + staged_ang_vel_des=zeros_3d.copy(), steps=steps, freq=freq, r_int_error=zeros_3d.copy(), @@ -570,7 +584,7 @@ class MellingerBodyRateData: cmd: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 4) """Body rate control command for the drone. - A command consists of [roll_rate, pitch_rate, yaw_rate, collective thrust]. + A command consists of [wx, wy, wz, collective thrust]. """ staged_cmd: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 4) """Staging buffer to store the most recent command until the next controller tick.""" @@ -650,7 +664,9 @@ def control_state2attitude(data: SimData) -> SimData: **state_ctrl.params, ) state_ctrl = leaf_replace(state_ctrl, mask, steps=data.core.steps, pos_err_i=pos_err_i) - attitude_ctrl = leaf_replace(data.controls.attitude, mask, staged_cmd=rpyt) + attitude_ctrl = leaf_replace( + data.controls.attitude, mask, staged_cmd=rpyt, staged_ang_vel_des=state_ctrl.cmd[..., 13:16] + ) return data.replace(controls=data.controls.replace(state=state_ctrl, attitude=attitude_ctrl)) @@ -660,14 +676,23 @@ def control_attitude2force_torque(data: SimData) -> SimData: attitude_ctrl: MellingerAttitudeData = data.controls.attitude assert attitude_ctrl is not None, "Using attitude controller without initialized data" mask = controllable(data.core.steps, data.core.freq, attitude_ctrl.steps, attitude_ctrl.freq) - attitude_ctrl = leaf_replace(attitude_ctrl, mask, cmd=attitude_ctrl.staged_cmd) - force, torque, r_int_error = attitude2force_torque( + prev_ang_vel_des = attitude_ctrl.ang_vel_des + attitude_ctrl = leaf_replace( + attitude_ctrl, + mask, + cmd=attitude_ctrl.staged_cmd, + ang_vel_des=attitude_ctrl.staged_ang_vel_des, + ) + force, torque, r_int_error = _attitude2force_torque( states.quat, states.ang_vel, - attitude_ctrl.cmd, - r_int_error=attitude_ctrl.r_int_error, - ctrl_freq=attitude_ctrl.freq, - prev_ang_vel=attitude_ctrl.last_ang_vel, + attitude_ctrl.cmd[..., :3], + attitude_ctrl.ang_vel_des, + attitude_ctrl.cmd[..., 3], + attitude_ctrl.last_ang_vel, + prev_ang_vel_des, + attitude_ctrl.r_int_error, + attitude_ctrl.freq, **attitude_ctrl.params, ) attitude_ctrl = leaf_replace( @@ -722,7 +747,12 @@ def control_commit_attitude(data: SimData) -> SimData: """Commit the staged attitude command to the controller setpoint.""" attitude_ctrl: MellingerAttitudeData = data.controls.attitude mask = controllable(data.core.steps, data.core.freq, attitude_ctrl.steps, attitude_ctrl.freq) - attitude_ctrl = leaf_replace(attitude_ctrl, mask, cmd=attitude_ctrl.staged_cmd) + attitude_ctrl = leaf_replace( + attitude_ctrl, + mask, + cmd=attitude_ctrl.staged_cmd, + ang_vel_des=attitude_ctrl.staged_ang_vel_des, + ) return data.replace(controls=data.controls.replace(attitude=attitude_ctrl)) diff --git a/crazyflow/sim/functional.py b/crazyflow/sim/functional.py index 7a592f2b..0bc193d3 100644 --- a/crazyflow/sim/functional.py +++ b/crazyflow/sim/functional.py @@ -16,7 +16,7 @@ def state_control(data: SimData, controls: Array) -> SimData: """State control function.""" assert data.controls.mode == Control.state, f"control type {data.controls.mode} not enabled" - assert controls.shape == (data.core.n_worlds, data.core.n_drones, 13), "controls shape mismatch" + assert controls.shape == (data.core.n_worlds, data.core.n_drones, 16), "controls shape mismatch" controls = jnp.asarray(controls) data = data.replace( controls=data.controls.replace(state=data.controls.state.replace(staged_cmd=controls)) diff --git a/docs/examples/index.md b/docs/examples/index.md index 20036a77..40cd8b0a 100644 --- a/docs/examples/index.md +++ b/docs/examples/index.md @@ -32,7 +32,7 @@ Commanding roll, pitch, yaw, and collective thrust directly. This level bypasses ## Body rate control -Commanding body-frame angular rates and collective thrust. The firmware controller has no dedicated body rate mode and levels the drone with its attitude terms, so the example sets the `kR` and `ki_m` gains of the body rate controller to zero. +Commanding body rates and collective thrust. The firmware controller has no dedicated body rate mode and levels the drone with its attitude terms, so the example sets the `kR` and `ki_m` gains of the body rate controller to zero. ```{ .python notest } diff --git a/docs/get-started/quick-start.md b/docs/get-started/quick-start.md index 745b13f4..3a3f17f9 100644 --- a/docs/get-started/quick-start.md +++ b/docs/get-started/quick-start.md @@ -17,19 +17,17 @@ sim.reset() ## State and command -The default control mode is `Control.state`. A state command is a 13-element vector that sets the desired position, velocity, acceleration, yaw, and body angular rates. +The default control mode is `Control.state`. A state command is a 16-element vector that sets the desired position, velocity, acceleration, attitude, and body rates. | Index | Variable | Units | |---|---|---| | 0–2 | Position \(x, y, z\) | m | | 3–5 | Velocity \(\dot{x}, \dot{y}, \dot{z}\) | m/s | | 6–8 | Acceleration \(\ddot{x}, \ddot{y}, \ddot{z}\) | m/s² | -| 9 | Yaw | rad | -| 10 | Roll rate | rad/s | -| 11 | Pitch rate | rad/s | -| 12 | Yaw rate | rad/s | +| 9–12 | Attitude quaternion \(q_x, q_y, q_z, q_w\) | | +| 13–15 | Body rates \(\omega_x, \omega_y, \omega_z\) | rad/s | -The command array has shape `(n_worlds, n_drones, 13)`. +Only the yaw of the attitude quaternion is used, as in the firmware. A zero quaternion is treated as zero yaw. The command array has shape `(n_worlds, n_drones, 16)`. ```python import numpy as np @@ -39,7 +37,7 @@ from crazyflow.control import Control sim = Sim(n_worlds=1, n_drones=1, freq=500, control=Control.state) sim.reset() -cmd = np.zeros((1, 1, 13), dtype=np.float32) +cmd = np.zeros((1, 1, 16), dtype=np.float32) cmd[0, 0, 2] = 0.5 # target height: 0.5 m ``` @@ -55,7 +53,7 @@ from crazyflow.control import Control sim = Sim(n_worlds=1, n_drones=1, freq=500, control=Control.state) sim.reset() -cmd = np.zeros((1, 1, 13), dtype=np.float32) +cmd = np.zeros((1, 1, 16), dtype=np.float32) cmd[0, 0, 2] = 0.5 for _ in range(10): @@ -75,7 +73,7 @@ from crazyflow.control import Control sim = Sim(n_worlds=1, n_drones=1, freq=500, control=Control.state) sim.reset() -cmd = np.zeros((1, 1, 13), dtype=np.float32) +cmd = np.zeros((1, 1, 16), dtype=np.float32) cmd[0, 0, 2] = 0.5 for _ in range(10): @@ -100,7 +98,7 @@ from crazyflow.control import Control sim = Sim(n_worlds=4, n_drones=1, freq=500, control=Control.state) sim.reset() -cmd = np.zeros((4, 1, 13), dtype=np.float32) +cmd = np.zeros((4, 1, 16), dtype=np.float32) cmd[:, 0, 2] = np.array([0.2, 0.4, 0.6, 0.8]) # different target heights per world for _ in range(10): @@ -112,7 +110,7 @@ pos = sim.data.states.pos[:, 0, :] # (4, 3) — position of drone 0 in each wor ## Simulate multiple drones -Increase `n_drones` to place multiple drones inside a single world. Each drone has its own independent state; all receive commands from the same `(n_worlds, n_drones, 13)` array. +Increase `n_drones` to place multiple drones inside a single world. Each drone has its own independent state; all receive commands from the same `(n_worlds, n_drones, 16)` array. ```python import numpy as np @@ -122,7 +120,7 @@ from crazyflow.control import Control sim = Sim(n_worlds=1, n_drones=4, freq=500, control=Control.state) sim.reset() -cmd = np.zeros((1, 4, 13), dtype=np.float32) +cmd = np.zeros((1, 4, 16), dtype=np.float32) cmd[0, :, 2] = np.array([0.2, 0.4, 0.6, 0.8]) # different height per drone for _ in range(10): diff --git a/docs/index.md b/docs/index.md index ac9f7616..d3349b4b 100644 --- a/docs/index.md +++ b/docs/index.md @@ -229,8 +229,8 @@ from crazyflow.control import Control sim = Sim(n_worlds=1, n_drones=1, control=Control.state) sim.reset() -# State command: [x, y, z, vx, vy, vz, ax, ay, az, yaw, roll_rate, pitch_rate, yaw_rate] -cmd = np.zeros((1, 1, 13), dtype=np.float32) +# State command: [x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, wx, wy, wz] +cmd = np.zeros((1, 1, 16), dtype=np.float32) cmd[0, 0, 2] = 0.5 # hover at 0.5 m sim.state_control(cmd) diff --git a/docs/user-guide/control/batching.md b/docs/user-guide/control/batching.md index 4d12c91c..2bdbf560 100644 --- a/docs/user-guide/control/batching.md +++ b/docs/user-guide/control/batching.md @@ -13,7 +13,7 @@ N = 100 pos = np.zeros((N, 3)) quat = np.tile(np.array([0.0, 0.0, 0.0, 1.0]), (N, 1)) vel = np.zeros((N, 3)) -cmd = np.zeros((N, 13)) +cmd = np.zeros((N, 16)) rpyt, int_pos_err = ctrl(pos, quat, vel, cmd) rpyt.shape # (100, 4) @@ -34,7 +34,7 @@ ctrl = parametrize(state2attitude, "cf2x_L250") pos = np.zeros((10, 5, 3)) quat = np.broadcast_to(np.array([0.0, 0.0, 0.0, 1.0]), (10, 5, 4)).copy() vel = np.zeros((10, 5, 3)) -cmd = np.zeros((10, 5, 13)) +cmd = np.zeros((10, 5, 16)) rpyt, _ = ctrl(pos, quat, vel, cmd) rpyt.shape # (10, 5, 4) diff --git a/docs/user-guide/control/controllers.md b/docs/user-guide/control/controllers.md index 8904e24a..da2d8f5a 100644 --- a/docs/user-guide/control/controllers.md +++ b/docs/user-guide/control/controllers.md @@ -12,7 +12,7 @@ The Mellinger controller [[1]](#references) is split into three stages that form | Stage | Function | Takes | Produces | |---|---|---|---| -| 1 | [`state2attitude`](mellinger.md#state-to-attitude) | State + 13-element setpoint | RPYT command + position integral error | +| 1 | [`state2attitude`](mellinger.md#state-to-attitude) | State + 16-element setpoint | RPYT command + position integral error | | 2 | [`attitude2force_torque`](mellinger.md#attitude-to-force-torque) | Attitude + RPYT command | Collective force, body torques + angular velocity integral error | | 3 | [`force_torque2rotor_vel`](mellinger.md#force-torque-to-rotor-velocities) | Force + torques | 4 motor speeds [RPM] | diff --git a/docs/user-guide/control/index.md b/docs/user-guide/control/index.md index 26a2392b..cf91fc03 100644 --- a/docs/user-guide/control/index.md +++ b/docs/user-guide/control/index.md @@ -4,12 +4,12 @@ Crazyflow provides five control modes, from high-level position setpoints down t ## Control hierarchy -Commands flow down a hierarchy. A state command is converted to an attitude command by the Mellinger controller; an attitude command is converted to force/torque by the geometric controller; force/torque is converted to rotor velocities by the mixer. Body rate control feeds the geometric controller with a rate setpoint instead of an attitude, so it enters the hierarchy at the same level as attitude control. +Commands flow down a hierarchy. A state command is converted to an attitude command by the Mellinger position controller, and its body rates are forwarded as the body rate setpoint; the attitude command and the body rate setpoint are converted to force/torque by the geometric controller; force/torque is converted to rotor velocities by the mixer. Body rate control feeds the geometric controller with a rate setpoint instead of an attitude, so it enters the hierarchy at the same level as attitude control. ``` State (13D) - └─ Mellinger controller - └─ Attitude (4D: roll, pitch, yaw, thrust) | Body rates (4D: ωx, ωy, ωz, thrust) + └─ Mellinger position controller + └─ Attitude (4D: roll, pitch, yaw, thrust) + body rates (3D: ωx, ωy, ωz) | Body rates (4D: ωx, ωy, ωz, thrust) └─ Geometric controller └─ Force/torque (4D: Fc, Tx, Ty, Tz) └─ Mixer @@ -28,19 +28,19 @@ sim = Sim(control=Control.state, state_freq=100, attitude_freq=500) sim.reset() ``` -Command shape: `(n_worlds, n_drones, 13)` +Command shape: `(n_worlds, n_drones, 16)` | Index | Variable | Units | |---|---|---| | 0–2 | Target position \(x, y, z\) | m | | 3–5 | Target velocity \(\dot{x}, \dot{y}, \dot{z}\) | m/s | | 6–8 | Target acceleration \(\ddot{x}, \ddot{y}, \ddot{z}\) | m/s² | -| 9 | Yaw | rad | -| 10 | Roll rate | rad/s | -| 11 | Pitch rate | rad/s | -| 12 | Yaw rate | rad/s | +| 9–12 | Attitude quaternion \(q_x, q_y, q_z, q_w\) | | +| 13–15 | Body rates \(\omega_x, \omega_y, \omega_z\) | rad/s | -Set unused elements to zero. A common hover command sets only the z position: +As in the firmware's full state setpoint, only the yaw of the attitude quaternion is used. The body rates are the angular velocity in the body frame. The position controller does not use them and forwards them to the attitude controller as its body rate setpoint. The fitted dynamics take the attitude command directly and ignore the body rates. + +Set unused elements to zero. A zero quaternion is treated as zero yaw. A common hover command sets only the z position: ```python import numpy as np @@ -50,7 +50,7 @@ from crazyflow.control import Control sim = Sim(control=Control.state) sim.reset() -cmd = np.zeros((1, 1, 13), dtype=np.float32) +cmd = np.zeros((1, 1, 16), dtype=np.float32) cmd[0, 0, 2] = 1.0 # hover at 1 m sim.state_control(cmd) @@ -96,7 +96,7 @@ sim.step(sim.freq // sim.control_freq) ## Body rate control -Commands body-frame angular rates and a collective thrust. The Mellinger controller tracks the rates with the same gains as in attitude control. As in the firmware, its attitude terms level the drone at the current yaw. Set the `kR` and `ki_m` parameters of the body rate controller to zero to track body rates without the levelling terms, see the [body rate example](../../examples/index.md#body-rate-control). Requires `Dynamics.first_principles`. +Commands body rates, i.e. the angular velocity in the body frame, and a collective thrust. The Mellinger controller tracks the rates with the same gains as in attitude control. As in the firmware, its attitude terms level the drone at the current yaw. Set the `kR` and `ki_m` parameters of the body rate controller to zero to track body rates without the levelling terms, see the [body rate example](../../examples/index.md#body-rate-control). Requires `Dynamics.first_principles`. ```python from crazyflow.sim import Sim, Dynamics @@ -110,9 +110,9 @@ Command shape: `(n_worlds, n_drones, 4)` | Index | Variable | Units | |---|---|---| -| 0 | Roll rate \(\omega_x\) | rad/s | -| 1 | Pitch rate \(\omega_y\) | rad/s | -| 2 | Yaw rate \(\omega_z\) | rad/s | +| 0 | Body rate \(\omega_x\) | rad/s | +| 1 | Body rate \(\omega_y\) | rad/s | +| 2 | Body rate \(\omega_z\) | rad/s | | 3 | Collective thrust | N | Zero rates and hover thrust hold the current attitude: diff --git a/docs/user-guide/control/integral-errors.md b/docs/user-guide/control/integral-errors.md index 6280cdde..5a4d1538 100644 --- a/docs/user-guide/control/integral-errors.md +++ b/docs/user-guide/control/integral-errors.md @@ -18,7 +18,7 @@ ctrl = parametrize(state2attitude, "cf2x_L250") pos = np.zeros(3) quat = np.array([0.0, 0.0, 0.0, 1.0]) vel = np.zeros(3) -cmd = np.zeros(13) +cmd = np.zeros(16) # Option 1: let the controller initialise the integral error. rpyt, pos_err_i = ctrl(pos, quat, vel, cmd, pos_err_i=None) @@ -42,7 +42,7 @@ ctrl = parametrize(state2attitude, "cf2x_L250") pos = np.zeros(3) quat = np.array([0.0, 0.0, 0.0, 1.0]) vel = np.zeros(3) -cmd = np.zeros(13) +cmd = np.zeros(16) cmd[0] = 1.0 # 1 m setpoint error in x pos_err_i = None @@ -68,7 +68,7 @@ pos = np.zeros(3) quat = np.array([0.0, 0.0, 0.0, 1.0]) vel = np.zeros(3) ang_vel = np.zeros(3) -cmd = np.zeros(13) +cmd = np.zeros(16) pos_err_i = None r_int_error = None diff --git a/docs/user-guide/control/jit.md b/docs/user-guide/control/jit.md index 7aa707fc..5f191e95 100644 --- a/docs/user-guide/control/jit.md +++ b/docs/user-guide/control/jit.md @@ -14,7 +14,7 @@ jit_ctrl = jax.jit(ctrl) pos = jnp.zeros(3) quat = jnp.array([0.0, 0.0, 0.0, 1.0]) vel = jnp.zeros(3) -cmd = jnp.zeros(13) +cmd = jnp.zeros(16) rpyt, int_pos_err = jit_ctrl(pos, quat, vel, cmd) ``` @@ -35,7 +35,7 @@ jit_ctrl = jax.jit(ctrl) pos = jnp.zeros(3) quat = jnp.array([0.0, 0.0, 0.0, 1.0]) vel = jnp.zeros(3) -cmd = jnp.zeros(13) +cmd = jnp.zeros(16) pos_err_i = jnp.zeros(3) # initialise to zero, so the function compiles only once for _ in range(10): @@ -59,7 +59,7 @@ N = 1_000 pos = jnp.zeros((N, 3)) quat = jnp.broadcast_to(jnp.array([0.0, 0.0, 0.0, 1.0]), (N, 4)) vel = jnp.zeros((N, 3)) -cmd = jnp.zeros((N, 13)) +cmd = jnp.zeros((N, 16)) rpyt, _ = jit_ctrl(pos, quat, vel, cmd) rpyt.shape # (1000, 4) diff --git a/docs/user-guide/control/mellinger.md b/docs/user-guide/control/mellinger.md index c31058ad..4482c48d 100644 --- a/docs/user-guide/control/mellinger.md +++ b/docs/user-guide/control/mellinger.md @@ -24,7 +24,7 @@ All three stages share the same state convention: | `pos` | `(..., 3)` | Current position [m] | | `quat` | `(..., 4)` | Current attitude, xyzw | | `vel` | `(..., 3)` | Current velocity [m/s] | -| `cmd` | `(..., 13)` | Setpoint: `[x, y, z, vx, vy, vz, ax, ay, az, yaw, avx, avy, avz]` | +| `cmd` | `(..., 16)` | Setpoint: `[x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, wx, wy, wz]`. Only the yaw of the quaternion `qx, qy, qz, qw` is used, as in the firmware. The body rates `wx, wy, wz` are not used by this stage, see below | | `pos_err_i` | `(..., 3)` or `None` | Position integral error from the previous call. `None` initialises to zero | | `ctrl_freq` | `float` | Control frequency in Hz (default 100) | @@ -35,6 +35,8 @@ All three stages share the same state convention: | `rpyt` | `(..., 4)` | Attitude + thrust: `[roll_rad, pitch_rad, yaw_rad, thrust_N]` | | `pos_err_i` | `(..., 3)` | Position integral error. Pass back as `pos_err_i` on the next call | +As in the firmware, the position controller does not use the body rates of the setpoint. They are the body rate setpoint of the attitude controller. `Control.state` forwards them to the attitude stage automatically. `attitude2force_torque` assumes a zero body rate setpoint. + ```python import numpy as np from crazyflow.control import parametrize @@ -45,7 +47,7 @@ ctrl = parametrize(state2attitude, "cf2x_L250") pos = np.zeros(3) quat = np.array([0.0, 0.0, 0.0, 1.0]) vel = np.zeros(3) -cmd = np.zeros(13) # setpoint at origin, yaw = 0 +cmd = np.zeros(16) # setpoint at origin, yaw = 0 rpyt, pos_err_i = ctrl(pos, quat, vel, cmd) rpyt.shape # (4,) @@ -101,7 +103,7 @@ torque.shape # (3,) |---|---|---| | `quat` | `(..., 4)` | Current attitude, xyzw | | `ang_vel` | `(..., 3)` | Current angular velocity in body frame [rad/s] | -| `cmd` | `(..., 4)` | Body rate command: `[roll_rate, pitch_rate, yaw_rate, thrust_N]` | +| `cmd` | `(..., 4)` | Body rate command: `[wx, wy, wz, thrust_N]` | | `prev_ang_vel` | `(..., 3)` or `None` | Angular velocity from the previous call. `None` initialises to zero | | `prev_cmd` | `(..., 4)` or `None` | Command from the previous call, used for the setpoint derivative. `None` assumes a constant setpoint | | `r_int_error` | `(..., 3)` or `None` | Angular velocity integral error from the previous call. `None` initialises to zero | @@ -125,7 +127,7 @@ params["kR"], params["ki_m"] = np.zeros(3), np.zeros(3) # pure body rate tracki quat = np.array([0.0, 0.0, 0.0, 1.0]) ang_vel = np.zeros(3) -cmd = np.array([0.5, 0.0, 0.0, 0.3]) # 0.5 rad/s roll rate, 0.3 N thrust +cmd = np.array([0.5, 0.0, 0.0, 0.3]) # 0.5 rad/s body rate about x, 0.3 N thrust force, torque, r_int_err = body_rate2force_torque(quat, ang_vel, cmd, **params) force.shape # (1,) @@ -182,7 +184,7 @@ pos = np.array([0.0, 0.0, 1.0]) # 1 m altitude quat = np.array([0.0, 0.0, 0.0, 1.0]) vel = np.zeros(3) ang_vel = np.zeros(3) -cmd = np.zeros(13) +cmd = np.zeros(16) cmd[:3] = np.array([0.0, 0.0, 1.0]) # hover at 1 m rpyt, _ = state_ctrl(pos, quat, vel, cmd) diff --git a/docs/user-guide/control/parametrize.md b/docs/user-guide/control/parametrize.md index 0795cea3..a3d743b1 100644 --- a/docs/user-guide/control/parametrize.md +++ b/docs/user-guide/control/parametrize.md @@ -29,7 +29,7 @@ ctrl = parametrize(state2attitude, "cf2x_L250") pos = np.zeros(3) quat = np.array([0.0, 0.0, 0.0, 1.0]) vel = np.zeros(3) -cmd = np.zeros(13) +cmd = np.zeros(16) # Simulate with a heavier drone for this call only. rpyt, _ = ctrl(pos, quat, vel, cmd, mass=0.035) @@ -71,7 +71,7 @@ ctrl = parametrize(state2attitude, "cf2x_L250") pos = np.zeros(3) quat = np.array([0.0, 0.0, 0.0, 1.0]) vel = np.zeros(3) -cmd = np.zeros(13) +cmd = np.zeros(16) rpyt, _ = ctrl(pos, quat, vel, cmd) ``` diff --git a/docs/user-guide/oo-api.md b/docs/user-guide/oo-api.md index 277ae22c..c0bd0357 100644 --- a/docs/user-guide/oo-api.md +++ b/docs/user-guide/oo-api.md @@ -50,7 +50,7 @@ All control methods take an array of shape `(n_worlds, n_drones, command_dim)` a ### State control -The highest-level interface. A 13-element command sets desired position, velocity, acceleration, yaw, and angular rates. An internal Mellinger controller converts this to attitude commands. +The highest-level interface. A 16-element command sets desired position, velocity, acceleration, attitude, and body rates. Only the yaw of the attitude quaternion is used, as in the firmware. An internal Mellinger controller converts this to attitude commands and forwards the body rates to the attitude controller. ```python import numpy as np @@ -60,8 +60,8 @@ from crazyflow.control import Control sim = Sim(n_worlds=1, n_drones=1, control=Control.state) sim.reset() -# [x, y, z, vx, vy, vz, ax, ay, az, yaw, roll_rate, pitch_rate, yaw_rate] -cmd = np.zeros((1, 1, 13), dtype=np.float32) +# [x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, wx, wy, wz] +cmd = np.zeros((1, 1, 16), dtype=np.float32) cmd[0, 0, 2] = 0.5 # hover at 0.5 m sim.state_control(cmd) @@ -90,7 +90,7 @@ sim.step(sim.freq // sim.control_freq) ### Body rate control -Commands body-frame angular rates (rad/s) and a collective thrust (N). The Mellinger controller tracks the rates and, as in the firmware, levels the drone with its attitude terms. Set `kR` and `ki_m` to zero for pure rate tracking, see [Control Modes](control/index.md#body-rate-control). Requires `Dynamics.first_principles`. +Commands body rates (rad/s) and a collective thrust (N). The Mellinger controller tracks the rates and, as in the firmware, levels the drone with its attitude terms. Set `kR` and `ki_m` to zero for pure rate tracking, see [Control Modes](control/index.md#body-rate-control). Requires `Dynamics.first_principles`. ```python import numpy as np @@ -169,7 +169,7 @@ sim = Sim(n_worlds=4, n_drones=1, control=Control.state) sim.reset() # reset all worlds # Stage a command and advance 50 dynamics steps (controllers fire at their rate) -cmd = np.zeros((4, 1, 13), dtype=np.float32) +cmd = np.zeros((4, 1, 16), dtype=np.float32) cmd[..., 2] = 0.5 sim.state_control(cmd) sim.step(50) @@ -193,7 +193,7 @@ from crazyflow.control import Control sim = Sim(n_worlds=2, n_drones=3, control=Control.state) sim.reset() -cmd = np.zeros((2, 3, 13), dtype=np.float32) +cmd = np.zeros((2, 3, 16), dtype=np.float32) for _ in range(10): sim.state_control(cmd) sim.step(sim.freq // sim.control_freq) diff --git a/docs/user-guide/sim-overview.md b/docs/user-guide/sim-overview.md index 997ed112..c491827f 100644 --- a/docs/user-guide/sim-overview.md +++ b/docs/user-guide/sim-overview.md @@ -75,7 +75,7 @@ from crazyflow.control import Control sim = Sim(freq=500, control=Control.state) sim.reset() -cmd = np.zeros((1, 1, 13), dtype=np.float32) +cmd = np.zeros((1, 1, 16), dtype=np.float32) sim.state_control(cmd) sim.step(sim.freq // sim.control_freq) # 500 // 100 = 5 dynamics steps, controller fires once ``` diff --git a/examples/contacts/crash.py b/examples/contacts/crash.py index ce8ae4b2..33640262 100644 --- a/examples/contacts/crash.py +++ b/examples/contacts/crash.py @@ -14,7 +14,7 @@ def main(): print("Phase 1: Hovering at [0, 0, 0.5] for 3 seconds") hover_duration = 3.0 - hover_cmd = np.zeros((sim.n_worlds, sim.n_drones, 13)) + hover_cmd = np.zeros((sim.n_worlds, sim.n_drones, 16)) hover_cmd[..., :3] = [0.0, 0.0, 0.5] # x, y, z position sim.state_control(hover_cmd) @@ -26,7 +26,7 @@ def main(): print("Phase 2: Dropping to [-5, 0, -0.5] for 3 seconds") drop_duration = 3.0 - drop_cmd = np.zeros((sim.n_worlds, sim.n_drones, 13)) + drop_cmd = np.zeros((sim.n_worlds, sim.n_drones, 16)) drop_cmd[..., :3] = [-5.0, 0.0, -0.5] # x, y, z position sim.state_control(drop_cmd) diff --git a/examples/control/attitude.py b/examples/control/attitude.py index 7c4053e2..ca58e1d1 100644 --- a/examples/control/attitude.py +++ b/examples/control/attitude.py @@ -30,11 +30,11 @@ def control( error. """ # Full state command with velocity and acceleration feedforward - cmd = np.zeros(13) + cmd = np.zeros(16) cmd[:3] = pos_start + np.array([np.cos(t) - 1, np.sin(t), 0.2 * t]) cmd[3:6] = np.array([-np.sin(t), np.cos(t), 0.2]) cmd[6:9] = np.array([-np.cos(t), -np.sin(t), 0.0]) - cmd[9] = t # Yaw + cmd[9:13] = np.array([0.0, 0.0, np.sin(t / 2), np.cos(t / 2)]) # Yaw quaternion return position_ctrl(obs["pos"], obs["quat"], obs["vel"], cmd, pos_err_i) diff --git a/examples/control/body_rate.py b/examples/control/body_rate.py index b92b1d59..19a8c52b 100644 --- a/examples/control/body_rate.py +++ b/examples/control/body_rate.py @@ -37,15 +37,14 @@ def control( pos_err_i: Integral error of the position controller from the previous call. Returns: - The body rate command [roll_rate, pitch_rate, yaw_rate, thrust] in rad/s and N, and the - updated integral error. + The body rate command [wx, wy, wz, thrust] in rad/s and N, and the updated integral error. """ # Full state command with velocity and acceleration feedforward - cmd = np.zeros(13) + cmd = np.zeros(16) cmd[:3] = pos_start + np.array([np.cos(t) - 1, np.sin(t), 0.2 * t]) cmd[3:6] = np.array([-np.sin(t), np.cos(t), 0.2]) cmd[6:9] = np.array([-np.cos(t), -np.sin(t), 0.0]) - cmd[9] = t # Yaw + cmd[9:13] = np.array([0.0, 0.0, np.sin(t / 2), np.cos(t / 2)]) # Yaw quaternion rpyt, pos_err_i = position_ctrl(obs["pos"], obs["quat"], obs["vel"], cmd, pos_err_i) rot_err = (R.from_quat(obs["quat"]).inv() * R.from_euler("xyz", rpyt[:3])).as_rotvec() return np.concatenate([kp_att * rot_err, rpyt[3:]]), pos_err_i @@ -69,7 +68,7 @@ def main(): # controller that outputs [roll, pitch, yaw, thrust], e.g. a learned policy. position_ctrl = partial(parametrize(state2attitude, sim.drone), ctrl_freq=sim.control_freq) pos_err_i = np.zeros(3) - cmd = np.zeros((sim.n_worlds, sim.n_drones, 4)) # [roll_rate, pitch_rate, yaw_rate, thrust] + cmd = np.zeros((sim.n_worlds, sim.n_drones, 4)) # [wx, wy, wz, thrust] pos_start = np.asarray(sim.data.states.pos[0, 0]) for i in range(int(duration * sim.control_freq)): # Convert the states to numpy so that the controller runs in numpy instead of eager JAX diff --git a/examples/control/change_pos.py b/examples/control/change_pos.py index 5c83940b..b5739530 100644 --- a/examples/control/change_pos.py +++ b/examples/control/change_pos.py @@ -18,7 +18,7 @@ def main(): rotor_vel=sim.data.states.rotor_vel.at[0, 0].set(np.ones(4) * 20000), ) ) - control = np.zeros((sim.n_worlds, sim.n_drones, 13)) + control = np.zeros((sim.n_worlds, sim.n_drones, 16)) control[..., :3] = np.array([[0.0, 0.0, 0.3]]) for _ in range(3 * sim.control_freq): diff --git a/examples/control/dynamics.py b/examples/control/dynamics.py index 98ace94f..215dfce9 100644 --- a/examples/control/dynamics.py +++ b/examples/control/dynamics.py @@ -15,7 +15,7 @@ def figure_eight(t: float) -> np.ndarray: """Return the position, velocity, and acceleration reference at time ``t``.""" omega = 2.0 * np.pi / DURATION phase = omega * t - cmd = np.zeros((1, 1, 13)) + cmd = np.zeros((1, 1, 16)) cmd[..., 0:3] = TRAJECTORY_CENTER + TRAJECTORY_SIZE * np.array( [np.sin(phase), 0.0, np.sin(2.0 * phase)] ) diff --git a/examples/control/hover.py b/examples/control/hover.py index c597e596..8a9f398a 100644 --- a/examples/control/hover.py +++ b/examples/control/hover.py @@ -20,8 +20,8 @@ def main(): duration = 5.0 fps = 60 - # State cmd is [x, y, z, vx, vy, vz, ax, ay, az, yaw, roll_rate, pitch_rate, yaw_rate] - cmd = np.zeros((sim.n_worlds, sim.n_drones, 13)) + # State cmd is [x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, wx, wy, wz] + cmd = np.zeros((sim.n_worlds, sim.n_drones, 16)) cmd[..., :3] = 0.1 for i in range(int(duration * sim.control_freq)): diff --git a/examples/control/spiral.py b/examples/control/spiral.py index 8f7b556a..fb657714 100644 --- a/examples/control/spiral.py +++ b/examples/control/spiral.py @@ -6,7 +6,7 @@ def control(start_xy: np.ndarray, t: float) -> np.ndarray: circle = np.array([np.cos(t) - 1, np.sin(t)]) - cmd = np.zeros((*start_xy.shape[:-1], 13)) + cmd = np.zeros((*start_xy.shape[:-1], 16)) cmd[..., :2] = start_xy + circle cmd[..., 2] = 0.2 * t return cmd diff --git a/examples/plugins/action_delay.py b/examples/plugins/action_delay.py index cc438d55..5c8bd618 100644 --- a/examples/plugins/action_delay.py +++ b/examples/plugins/action_delay.py @@ -19,7 +19,7 @@ def control(t: float) -> np.ndarray: - cmd = np.zeros((1, 1, 13)) + cmd = np.zeros((1, 1, 16)) cmd[..., :3] = [np.cos(t) - 1, np.sin(t), 0.2 * t] return cmd diff --git a/examples/plugins/disturbance.py b/examples/plugins/disturbance.py index bd692b86..7b2276ae 100644 --- a/examples/plugins/disturbance.py +++ b/examples/plugins/disturbance.py @@ -28,7 +28,7 @@ def disturbance_fn(data: SimData) -> SimData: def main(plot: bool = False): sim = Sim(control="state") - control = np.zeros((sim.n_worlds, sim.n_drones, 13)) + control = np.zeros((sim.n_worlds, sim.n_drones, 16)) control[..., :3] = 0.2 # First run diff --git a/examples/plugins/estimation.py b/examples/plugins/estimation.py index 1686129d..7e4724f3 100644 --- a/examples/plugins/estimation.py +++ b/examples/plugins/estimation.py @@ -40,7 +40,7 @@ def trajectory(t: float, t_total: float = 20.0) -> np.ndarray: center = np.array([0.0, 0.0, 1.0]) size = np.array([1.0, 0.75, 0.0]) omega = 2 * np.pi / t_total - cmd = np.zeros((1, 1, 13)) + cmd = np.zeros((1, 1, 16)) pos = center + size * np.array([np.sin(omega * t), np.sin(2 * omega * t), 0.0]) vel = size * omega * np.array([np.cos(omega * t), 2 * np.cos(2 * omega * t), 0.0]) acc = size * omega**2 * np.array([-np.sin(omega * t), -4 * np.sin(2 * omega * t), 0.0]) diff --git a/examples/plugins/randomize.py b/examples/plugins/randomize.py index ce4d6212..d8c3ff24 100644 --- a/examples/plugins/randomize.py +++ b/examples/plugins/randomize.py @@ -138,7 +138,7 @@ def main(): fps = 60 for _ in range(3): - cmd = np.zeros((sim.n_worlds, sim.n_drones, 13)) + cmd = np.zeros((sim.n_worlds, sim.n_drones, 16)) cmd[..., 2] = 0.4 cmd[..., :2] = grid_2d(sim.n_drones) * 0.25 diff --git a/examples/rendering/cam_config.py b/examples/rendering/cam_config.py index f70d64d5..8011068f 100644 --- a/examples/rendering/cam_config.py +++ b/examples/rendering/cam_config.py @@ -14,7 +14,7 @@ def main(cam_config: dict | None = None): duration = 5.0 fps = 60 - cmd = np.zeros((sim.n_worlds, sim.n_drones, 13)) + cmd = np.zeros((sim.n_worlds, sim.n_drones, 16)) cmd[..., :3] = 0.2 for i in range(int(duration * sim.control_freq)): diff --git a/examples/rendering/cameras.py b/examples/rendering/cameras.py index c07b59eb..83e7069b 100644 --- a/examples/rendering/cameras.py +++ b/examples/rendering/cameras.py @@ -16,7 +16,7 @@ def control(t: float, t_tot: float) -> np.ndarray: phi = 2 * np.pi * t / t_tot + np.pi circle = np.array([np.cos(phi), np.sin(phi)]) - cmd = np.zeros((1, 1, 13)) + cmd = np.zeros((1, 1, 16)) cmd[..., :2] = circle # xy cmd[..., 2] = 0.1 + 0.5 * t / t_tot # z cmd[..., -4] = 1.9 * np.pi * t / t_tot # yaw diff --git a/examples/rendering/led_deck.py b/examples/rendering/led_deck.py index ba427145..36ee1ab9 100644 --- a/examples/rendering/led_deck.py +++ b/examples/rendering/led_deck.py @@ -50,7 +50,7 @@ def main(): rgbas[..., 3] = 1.0 init_pos = np.array(sim.data.states.pos[0, :, :]) - cmd = np.zeros((sim.n_worlds, sim.n_drones, 13)) + cmd = np.zeros((sim.n_worlds, sim.n_drones, 16)) cmd[:, :, :3] = init_pos cmd[:, :, 2] += 1.5 diff --git a/examples/rendering/splat_camera.py b/examples/rendering/splat_camera.py index d24e7ed7..b80f8a31 100644 --- a/examples/rendering/splat_camera.py +++ b/examples/rendering/splat_camera.py @@ -30,11 +30,12 @@ def control(t: float, n_worlds: int, n_drones: int) -> np.ndarray: """Circle both drones around the center, each yawed to look across at its partner.""" drones = np.arange(n_drones)[None, :] angle = t + (2 * np.pi / n_drones) * drones - cmd = np.zeros((n_worlds, n_drones, 13)) + cmd = np.zeros((n_worlds, n_drones, 16)) cmd[..., 0] = RADIUS * np.cos(angle) cmd[..., 1] = RADIUS * np.sin(angle) cmd[..., 2] = HEIGHT - cmd[..., 9] = angle + np.pi # yaw faces the opposite point on the circle, where the partner is + yaw = angle + np.pi # yaw faces the opposite point on the circle, where the partner is + cmd[..., 11], cmd[..., 12] = np.sin(yaw / 2), np.cos(yaw / 2) return cmd diff --git a/examples/rendering/splat_depth.py b/examples/rendering/splat_depth.py index ba15232b..6b90b1a1 100644 --- a/examples/rendering/splat_depth.py +++ b/examples/rendering/splat_depth.py @@ -42,10 +42,11 @@ def control(t: float) -> np.ndarray: A state command placing the drone on the ellipse with its yaw along the tangent. """ angle = 2 * np.pi * t - cmd = np.zeros((1, 1, 13)) + cmd = np.zeros((1, 1, 16)) cmd[..., :2] = CENTER + RADII * np.array([np.cos(angle), np.sin(angle)]) cmd[..., 2] = HEIGHT - cmd[..., 9] = np.arctan2(RADII[1] * np.cos(angle), -RADII[0] * np.sin(angle)) + yaw = np.arctan2(RADII[1] * np.cos(angle), -RADII[0] * np.sin(angle)) + cmd[..., 9:13] = R.from_euler("z", yaw).as_quat() return cmd @@ -60,7 +61,7 @@ def main(show_plot: bool = False): # Start on the lap so the controller does not have to fly in from the origin first cmd = control(0.0) states = sim.data.states.replace(pos=sim.data.states.pos.at[..., :].set(cmd[..., :3])) - states = states.replace(quat=states.quat.at[...].set(R.from_euler("z", cmd[..., 9]).as_quat())) + states = states.replace(quat=states.quat.at[...].set(cmd[..., 9:13])) sim.data = sim.data.replace(states=states) fig, ax = plt.subplots(figsize=(7, 5)) diff --git a/examples/rendering/splat_viewer.py b/examples/rendering/splat_viewer.py index 9ad3646d..c70bffee 100644 --- a/examples/rendering/splat_viewer.py +++ b/examples/rendering/splat_viewer.py @@ -25,7 +25,7 @@ def control(t: float) -> np.ndarray: - cmd = np.zeros((1, 1, 13)) + cmd = np.zeros((1, 1, 16)) cmd[..., :3] = [0.5 * (np.cos(t) - 1), 0.5 * np.sin(t), 1.0 + 0.2 * np.sin(0.5 * t)] return cmd diff --git a/tests/integration/test_disturbance.py b/tests/integration/test_disturbance.py index 359c9584..5d9336fa 100644 --- a/tests/integration/test_disturbance.py +++ b/tests/integration/test_disturbance.py @@ -19,7 +19,7 @@ def disturbance_fn(data: SimData) -> SimData: @pytest.mark.integration def test_disturbance(dynamics: Dynamics): sim = Sim(n_worlds=2, n_drones=3, control="state", dynamics=dynamics) - control = np.zeros((sim.n_worlds, sim.n_drones, 13)) + control = np.zeros((sim.n_worlds, sim.n_drones, 16)) control[..., :3] = 1.0 n_steps = 10 diff --git a/tests/integration/test_interfaces.py b/tests/integration/test_interfaces.py index 17b5c26c..cac608e7 100644 --- a/tests/integration/test_interfaces.py +++ b/tests/integration/test_interfaces.py @@ -17,7 +17,7 @@ def test_state_interface(dynamics: Dynamics): # Simple P controller for attitude to reach target height target_height = 0.5 - cmd = np.zeros((1, 1, 13), dtype=np.float32) + cmd = np.zeros((1, 1, 16), dtype=np.float32) cmd[0, 0, 2] = target_height steps = int(2 * sim.control_freq) # Run simulation for 2 seconds @@ -31,6 +31,31 @@ def test_state_interface(dynamics: Dynamics): assert distance < 0.1, f"Failed to reach target height with {dynamics} dynamics" +@pytest.mark.integration +def test_state_interface_body_rate_feedforward(): + """The body rates of the state command must reach the attitude controller as feedforward.""" + yaw_rate = 1.5 + + def yaw_ramp_error(feedforward: bool) -> float: + sim = Sim(dynamics=Dynamics.first_principles, control=Control.state) + cmd = np.zeros((1, 1, 16)) + cmd[..., 2] = 0.5 + errors = [] + for i in range(int(4 * sim.control_freq)): + yaw_des = yaw_rate * i / sim.control_freq + cmd[..., 9:13] = R.from_euler("z", yaw_des).as_quat() + cmd[..., 15] = yaw_rate if feedforward else 0.0 + sim.state_control(cmd) + sim.step(sim.freq // sim.control_freq) + yaw = R.from_quat(sim.data.states.quat[0, 0]).as_euler("xyz")[2] + errors.append(abs((yaw - yaw_des + np.pi) % (2 * np.pi) - np.pi)) + return np.mean(errors[sim.control_freq :]) # Skip the first second of transients + + error_ff, error_no_ff = yaw_ramp_error(True), yaw_ramp_error(False) + assert error_ff < 0.05, f"Yaw lag with body rate feedforward: {error_ff}" + assert error_ff < error_no_ff / 2, f"Feedforward must reduce the yaw lag ({error_no_ff})" + + @pytest.mark.integration @pytest.mark.parametrize("dynamics", Dynamics) def test_attitude_interface(dynamics: Dynamics): @@ -39,7 +64,7 @@ def test_attitude_interface(dynamics: Dynamics): jit_state2attitude = jax.jit(parametrize(state2attitude, drone=sim.drone)) pos_err_i = np.zeros((1, 1, 3)) - cmd = np.zeros((1, 1, 13)) + cmd = np.zeros((1, 1, 16)) cmd[0, 0, 2] = 1.0 steps = int(3 * sim.control_freq) @@ -69,7 +94,7 @@ def test_body_rate_interface(): kp_att = 8.0 # Proportional gain from attitude error to body rates pos_err_i = np.zeros((1, 1, 3)) - cmd = np.zeros((1, 1, 13)) + cmd = np.zeros((1, 1, 16)) body_rate_cmd = np.zeros((1, 1, 4)) steps = int(3 * sim.control_freq) @@ -116,7 +141,7 @@ def test_swarm_control(dynamics: Dynamics): sim = Sim(n_worlds=n_worlds, n_drones=n_drones, dynamics=dynamics, control=Control.state) start_pos = np.asarray(sim.data.states.pos) target_pos = sim.data.states.pos + np.array([0.3, 0.3, 0.3]) - cmd = np.zeros((n_worlds, n_drones, 13)) + cmd = np.zeros((n_worlds, n_drones, 16)) steps = int(3 * sim.control_freq) for i in range(steps): @@ -138,9 +163,9 @@ def test_yaw_rotation(dynamics: Dynamics): sim = Sim(dynamics=dynamics, control=Control.state, state_freq=100) sim.reset() - cmd = np.zeros((sim.n_worlds, sim.n_drones, 13)) + cmd = np.zeros((sim.n_worlds, sim.n_drones, 16)) cmd[..., :3] = 0.2 - cmd[..., 9] = np.pi / 2 # Test if the drone can rotate in yaw + cmd[..., 9:13] = R.from_euler("z", np.pi / 2).as_quat() # Test if the drone can rotate in yaw sim.state_control(cmd) sim.step(200 * sim.freq // sim.control_freq) # Run simulation for 2 seconds diff --git a/tests/unit/control/test_mellinger.py b/tests/unit/control/test_mellinger.py index b4a518db..c169edee 100644 --- a/tests/unit/control/test_mellinger.py +++ b/tests/unit/control/test_mellinger.py @@ -30,12 +30,12 @@ def test_state2attitude(drone: str) -> None: controller = parametrize(state2attitude, drone) # Single input pos, quat, vel, ang_vel = create_rnd_states() - rpyt, pos_err_i = controller(pos, quat, vel, np.ones(13), ctrl_freq=100) + rpyt, pos_err_i = controller(pos, quat, vel, np.ones(16), ctrl_freq=100) assert rpyt.shape == (4,) assert pos_err_i.shape == (3,) # Batch input pos, quat, vel, ang_vel = create_rnd_states((5, 4)) - rpyt, pos_err_i = controller(pos, quat, vel, np.ones((5, 4, 13)), ctrl_freq=100) + rpyt, pos_err_i = controller(pos, quat, vel, np.ones((5, 4, 16)), ctrl_freq=100) assert rpyt.shape == (5, 4, 4) assert pos_err_i.shape == (5, 4, 3) @@ -67,7 +67,7 @@ def test_body_rate2force_torque(drone: str) -> None: controller = parametrize(body_rate2force_torque, drone) # Single input _, quat, _, ang_vel = create_rnd_states() - cmd = np.array([0.1, 0.1, 0.1, 1.0]) # roll rate, pitch rate, yaw rate, thrust command + cmd = np.array([0.1, 0.1, 0.1, 1.0]) # body rates and thrust command force_des, torque_des, r_int_error = controller(quat, ang_vel, cmd) assert force_des.shape == (1,) assert torque_des.shape == (3,) @@ -110,7 +110,7 @@ def test_state2attitude_at_setpoint(drone: str) -> None: pos = np.zeros(3) quat = np.array([0.0, 0.0, 0.0, 1.0]) vel = np.zeros(3) - cmd = np.zeros(13) # setpoint at origin, zero vel/acc, yaw=0 + cmd = np.zeros(16) # setpoint at origin, zero vel/acc, yaw=0 rpyt, _ = controller(pos, quat, vel, cmd) assert np.allclose(rpyt[:3], 0.0, atol=1e-6), f"RPY at setpoint should be ~0, got {rpyt[:3]}" assert rpyt[3] > 0.0, "Hovering thrust must be positive" @@ -126,7 +126,7 @@ def test_state2attitude_integral_error_accumulation(drone: str) -> None: pos = np.zeros(3) quat = np.array([0.0, 0.0, 0.0, 1.0]) vel = np.zeros(3) - cmd = np.zeros(13) + cmd = np.zeros(16) cmd[0] = 1.0 # 1 m setpoint error in x ctrl_freq = 100.0 dt = 1.0 / ctrl_freq @@ -261,7 +261,7 @@ def test_state2attitude_batch_consistency(drone: str): controller = parametrize(state2attitude, drone) batch = (3, 2) pos, quat, vel, _ = create_rnd_states(batch) - cmd = np.random.randn(*batch, 13) + cmd = np.random.randn(*batch, 16) rpyt_batch, err_batch = controller(pos, quat, vel, cmd) for i in range(batch[0]): for j in range(batch[1]): diff --git a/tests/unit/test_functional.py b/tests/unit/test_functional.py index 77a02dbb..9594a60d 100644 --- a/tests/unit/test_functional.py +++ b/tests/unit/test_functional.py @@ -123,7 +123,7 @@ def test_functional_state_control(state_freq: int): can_control_2 = np.array([0, 0, 1, 2, 3, 4]) * state_freq % sim.freq < state_freq for i in range(6): - cmd = np.random.rand(sim.n_worlds, sim.n_drones, 13) + cmd = np.random.rand(sim.n_worlds, sim.n_drones, 16) # Check controllable status controllable = F.controllable(data) assert jnp.all(controllable[0] == can_control_1[i]), f"Controllable 1 mismatch at t={i}" @@ -160,7 +160,7 @@ def test_functional_state_control_device(device: str): """Test that functional state control maintains JAX arrays on correct device.""" sim = Sim(n_worlds=2, n_drones=3, control=Control.state, device=device) data = sim.build_data() - cmd = np.random.rand(sim.n_worlds, sim.n_drones, 13) + cmd = np.random.rand(sim.n_worlds, sim.n_drones, 16) data = F.state_control(data, cmd) controls = data.controls.state assert isinstance(controls.cmd, jnp.ndarray), "Buffers must remain JAX arrays" diff --git a/tests/unit/test_gradients.py b/tests/unit/test_gradients.py index 3209cf78..580b901f 100644 --- a/tests/unit/test_gradients.py +++ b/tests/unit/test_gradients.py @@ -25,7 +25,7 @@ def height(cmd: Array, data: SimData) -> Array: ) return sim_step(data, sim.freq // sim.control_freq).states.pos[0, 0, 2] - cmd = jnp.zeros((1, 1, 13), dtype=jnp.float32) + cmd = jnp.zeros((1, 1, 16), dtype=jnp.float32) cmd = cmd.at[..., 2].set(1.01) grad = jax.jit(jax.grad(height))(cmd, data) diff --git a/tests/unit/test_sim.py b/tests/unit/test_sim.py index 6d5cabda..b3aa2b8f 100644 --- a/tests/unit/test_sim.py +++ b/tests/unit/test_sim.py @@ -78,8 +78,8 @@ def test_sim_init(dynamics: Dynamics, device: str, control: Control, n_worlds: i # Test control buffer shapes if control == Control.state: assert isinstance(sim.data.controls.state, ControlData) - array_meta_assert(sim.data.controls.state.staged_cmd, (n_worlds, n_drones, 13), device) - array_meta_assert(sim.data.controls.state.cmd, (n_worlds, n_drones, 13), device) + array_meta_assert(sim.data.controls.state.staged_cmd, (n_worlds, n_drones, 16), device) + array_meta_assert(sim.data.controls.state.cmd, (n_worlds, n_drones, 16), device) else: assert sim.data.controls.state is None # Test attitude buffer shapes @@ -227,6 +227,22 @@ def test_sim_step(n_worlds: int, n_drones: int, dynamics: Dynamics, control: Con sim.step(2) +@pytest.mark.unit +def test_state_control_forwards_body_rates(): + """State control must forward the body rates of the command to the attitude controller.""" + sim = Sim(n_worlds=2, n_drones=3, control=Control.state) + cmd = np.zeros((sim.n_worlds, sim.n_drones, 16)) + cmd[..., 13:16] = np.random.rand(sim.n_worlds, sim.n_drones, 3) + sim.state_control(cmd) + sim.step() + assert np.allclose(sim.data.controls.attitude.ang_vel_des, cmd[..., 13:16]) + # Attitude control never sets a body rate setpoint + sim = Sim(n_worlds=2, n_drones=3, control=Control.attitude) + sim.attitude_control(np.random.rand(sim.n_worlds, sim.n_drones, 4)) + sim.step() + assert np.all(sim.data.controls.attitude.ang_vel_des == 0.0) + + @pytest.mark.unit @pytest.mark.parametrize("attitude_freq", [33, 50, 100, 200]) def test_sim_attitude_control(attitude_freq: int): @@ -271,7 +287,7 @@ def test_sim_state_control(state_freq: int): can_control_1 = np.arange(6) * state_freq % sim.freq < state_freq can_control_2 = np.array([0, 0, 1, 2, 3, 4]) * state_freq % sim.freq < state_freq for i in range(6): - cmd = np.random.rand(sim.n_worlds, sim.n_drones, 13) + cmd = np.random.rand(sim.n_worlds, sim.n_drones, 16) assert jnp.all(sim.controllable[0] == can_control_1[i]), f"Controllable 1 mismatch at t={i}" assert jnp.all(sim.controllable[1] == can_control_2[i]), f"Controllable 2 mismatch at t={i}" sim.state_control(cmd) @@ -295,7 +311,7 @@ def test_sim_state_control(state_freq: int): @pytest.mark.unit def test_sim_state_control_device(device: str): sim = Sim(n_worlds=2, n_drones=3, control=Control.state, device=device) - cmd = np.random.rand(sim.n_worlds, sim.n_drones, 13) + cmd = np.random.rand(sim.n_worlds, sim.n_drones, 16) sim.state_control(cmd) controls = sim.data.controls.state assert isinstance(controls.cmd, jnp.ndarray), "Buffers must remain JAX arrays" @@ -348,7 +364,7 @@ def test_control_frequency(dynamics: Dynamics): sim_1000 = Sim(freq=1000, dynamics=dynamics, control="state") # Set same initial state and controls - cmd = np.zeros((1, 1, 13)) # Single world, single drone, state control + cmd = np.zeros((1, 1, 16)) # Single world, single drone, state control # Target position of (1, 1, 1). Needs to be off-center to check attitude integration error cmd[..., :3] = 1.0 @@ -634,7 +650,7 @@ def test_compile(dynamics: Dynamics, device: str): def test_scan_results(dynamics: Dynamics): sim = Sim(n_worlds=2, n_drones=3, dynamics=dynamics, control=Control.state, device="cpu") sim.reset() - cmd = np.zeros((sim.n_worlds, sim.n_drones, 13)) + cmd = np.zeros((sim.n_worlds, sim.n_drones, 16)) cmd[..., :3] = sim.data.states.pos + np.array([0.3, 0.3, 0.3]) sim.state_control(cmd) n_steps, n_iters = sim.freq // sim.control_freq, 100 # 1 second at 100Hz From 5319c93ce753c8b4598ae9e552843c2545858a0a Mon Sep 17 00:00:00 2001 From: ratheron Date: Mon, 14 Sep 2026 18:34:42 +0200 Subject: [PATCH 4/6] Update docs --- README.md | 2 ++ crazyflow/control/core.py | 6 ------ crazyflow/control/mellinger/control.py | 3 ++- docs/user-guide/control/index.md | 2 +- examples/rendering/cameras.py | 2 +- 5 files changed, 6 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index 9ec63eea..bc589ed9 100644 --- a/README.md +++ b/README.md @@ -18,9 +18,11 @@ Crazyflow is a research simulator for quadrotors. It runs batched, differentiabl import numpy as np from crazyflow.sim import Sim from crazyflow.control import Control +from scipy.spatial.transform import Rotation as R sim = Sim(n_worlds=4096, n_drones=1, control=Control.state) cmd = np.zeros((4096, 1, 16)) +cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat() cmd[..., 2] = 0.5 # hover at 0.5 m across all worlds for _ in range(100): diff --git a/crazyflow/control/core.py b/crazyflow/control/core.py index a2d8018b..6c18d67e 100644 --- a/crazyflow/control/core.py +++ b/crazyflow/control/core.py @@ -89,10 +89,6 @@ class Control(StrEnum): state = "state" """State control takes [x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, wx, wy, wz]. - The attitude setpoint qx, qy, qz, qw is an xyzw quaternion. The body rates wx, wy, wz are the - angular velocity in the body frame in rad/s. The position controller forwards them to the - attitude controller as its body rate setpoint. - Note: Recommended frequency is >=20 Hz. @@ -109,8 +105,6 @@ class Control(StrEnum): body_rate = "body_rate" """Body rate control takes [wx, wy, wz, collective thrust]. - The body rates wx, wy, wz are the angular velocity in the body frame in rad/s. - Note: Recommended frequency is >=200 Hz. """ diff --git a/crazyflow/control/mellinger/control.py b/crazyflow/control/mellinger/control.py index b6ddcf47..8f38808b 100644 --- a/crazyflow/control/mellinger/control.py +++ b/crazyflow/control/mellinger/control.py @@ -101,7 +101,8 @@ def state2attitude( target_thrust = ( mass * (setpoint_acc - gravity_vec) + kp * pos_err + kd * vel_err + ki * int_pos_err ) - # l. 166 ff Only the yaw of the setpoint attitude is used, as in the firmware + # l. 178 Rate-controlled YAW is moving YAW angle setpoint + # => only one case here, since the setpoint is always in absolute mode desired_yaw = R.from_quat(setpoint_quat).as_euler("xyz")[..., 2] # l. 189 Z-Axis [zB] rot = R.from_quat(quat).as_matrix() diff --git a/docs/user-guide/control/index.md b/docs/user-guide/control/index.md index c8e04855..75adeecf 100644 --- a/docs/user-guide/control/index.md +++ b/docs/user-guide/control/index.md @@ -4,7 +4,7 @@ Crazyflow provides five control modes, from high-level position setpoints down t ## Control hierarchy -Commands flow down a hierarchy. A state command is converted to an attitude command by the Mellinger position controller, and its body rates are forwarded as the body rate setpoint; the attitude command and the body rate setpoint are converted to force/torque by the geometric controller; force/torque is converted to rotor velocities by the mixer. Body rate control feeds the geometric controller with a rate setpoint instead of an attitude, so it enters the hierarchy at the same level as attitude control. +Commands flow down a hierarchy. A state command is converted to an attitude command by the Mellinger position controller; the attitude command is converted to force/torque by the geometric controller; force/torque is converted to rotor velocities by the mixer. Body rate control feeds the geometric controller with a rate setpoint instead of an attitude, so it enters the hierarchy at the same level as attitude control. The rate setpoint in the state command is forwarded to the rate controller. ``` State (16D) diff --git a/examples/rendering/cameras.py b/examples/rendering/cameras.py index 13d36e66..4e09f718 100644 --- a/examples/rendering/cameras.py +++ b/examples/rendering/cameras.py @@ -20,7 +20,7 @@ def control(t: float, t_tot: float) -> np.ndarray: phi = 2 * np.pi * t / t_tot + np.pi circle = np.array([np.cos(phi), np.sin(phi)]) - yaw = 1.9 * np.pi * t / t_tot + yaw = 2 * np.pi * t / t_tot cmd = np.zeros((1, 1, 16)) cmd[..., :2] = circle # xy cmd[..., 2] = 0.1 + 0.5 * t / t_tot # z From 58911bbd1c5526edd72e3173917ce84564bcd3ac Mon Sep 17 00:00:00 2001 From: ratheron Date: Mon, 14 Sep 2026 20:40:33 +0200 Subject: [PATCH 5/6] Rename parameters, simiplify docstring --- crazyflow/control/mellinger/control.py | 49 +++++++++----------------- docs/user-guide/control/mellinger.md | 4 +-- docs/user-guide/functional-api.md | 2 +- docs/user-guide/oo-api.md | 2 +- tests/unit/test_functional.py | 14 ++++---- tests/unit/test_sim.py | 14 ++++---- 6 files changed, 35 insertions(+), 50 deletions(-) diff --git a/crazyflow/control/mellinger/control.py b/crazyflow/control/mellinger/control.py index 8f38808b..faa1b142 100644 --- a/crazyflow/control/mellinger/control.py +++ b/crazyflow/control/mellinger/control.py @@ -59,11 +59,8 @@ def state2attitude( quat: Drone orientation as xyzw quaternion with shape (..., 4). vel: Drone velocity with shape (..., 3). cmd: Full state command in SI units with shape (..., 16). The entries are - [x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, wx, wy, wz], where qx, qy, qz, qw is - the attitude setpoint as xyzw quaternion and wx, wy, wz are the body rates in rad/s. As - in the firmware, only the yaw of the attitude setpoint is used. The body rates are not - used by the position controller. They are the body rate setpoint of the attitude - controller. + [x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, wx, wy, wz]. Only the yaw of the + attitude quaternion is used. The body rates are forwarded to the attitude controller. pos_err_i: Position integral error (..., 3) from the previous call. If None, it is initialised to zero. ctrl_freq: Control frequency in Hz @@ -495,9 +492,8 @@ class MellingerStateData: cmd: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 16) """Full state control command for the drone. - A command consists of [x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, wx, wy, wz]. Only the - yaw of the attitude quaternion is used. The body rates wx, wy, wz are forwarded to the attitude - controller as its body rate setpoint. + A command consists of [x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, wx, wy, wz]. Only the + yaw of the attitude quaternion is used. The body rates are forwarded to the attitude controller. """ staged_cmd: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 16) """Staging buffer to store the most recent command until the next controller tick.""" @@ -532,17 +528,11 @@ def create( @dataclass class MellingerAttitudeData: cmd: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 4) - """Full attitude control command for the drone. - - A command consists of [roll, pitch, yaw, collective thrust]. - """ + """Attitude control setpoint consisting of [roll, pitch, yaw, collective thrust].""" staged_cmd: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 4) """Staging buffer to store the most recent command until the next controller tick.""" ang_vel_des: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 3) - """Body rate setpoint [wx, wy, wz] of the attitude controller. - - Zero for attitude control. State control forwards the body rates of the state command. - """ + """Body rate setpoint [wx, wy, wz] of the attitude controller.""" staged_ang_vel_des: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 3) """Staging buffer to store the most recent body rate setpoint until the next controller tick.""" steps: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, 1) @@ -551,8 +541,8 @@ class MellingerAttitudeData: """Frequency of the attitude control command.""" r_int_error: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 3) """Integral errors of the attitude control command.""" - last_ang_vel: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 3) - """Last angular velocity of the drone.""" + prev_ang_vel: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 3) + """Previous angular velocity of the drone.""" # Parameters for the attitude controller params: dict[str, Array] @@ -573,7 +563,7 @@ def create( steps=steps, freq=freq, r_int_error=zeros_3d.copy(), - last_ang_vel=zeros_3d.copy(), + prev_ang_vel=zeros_3d.copy(), params=params, ) @@ -593,8 +583,8 @@ class MellingerBodyRateData: """Frequency of the body rate control command.""" r_int_error: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 3) """Integral errors of the body rate control command.""" - last_ang_vel: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 3) - """Last angular velocity of the drone.""" + prev_ang_vel: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 3) + """Previous angular velocity of the drone.""" # Parameters for the body rate controller params: dict[str, Array] @@ -613,7 +603,7 @@ def create( steps=steps, freq=freq, r_int_error=zeros_3d.copy(), - last_ang_vel=zeros_3d.copy(), + prev_ang_vel=zeros_3d.copy(), params=params, ) @@ -688,7 +678,7 @@ def control_attitude2force_torque(data: SimData) -> SimData: attitude_ctrl.cmd[..., :3], attitude_ctrl.ang_vel_des, attitude_ctrl.cmd[..., 3], - attitude_ctrl.last_ang_vel, + attitude_ctrl.prev_ang_vel, prev_ang_vel_des, attitude_ctrl.r_int_error, attitude_ctrl.freq, @@ -698,7 +688,7 @@ def control_attitude2force_torque(data: SimData) -> SimData: attitude_ctrl, mask, r_int_error=r_int_error, - last_ang_vel=states.ang_vel, + prev_ang_vel=states.ang_vel, steps=data.core.steps, ) ft_ctrl = leaf_replace( @@ -721,7 +711,7 @@ def control_body_rate2force_torque(data: SimData) -> SimData: states.quat, states.ang_vel, body_rate_ctrl.cmd, - prev_ang_vel=body_rate_ctrl.last_ang_vel, + prev_ang_vel=body_rate_ctrl.prev_ang_vel, prev_cmd=prev_cmd, r_int_error=body_rate_ctrl.r_int_error, ctrl_freq=body_rate_ctrl.freq, @@ -731,7 +721,7 @@ def control_body_rate2force_torque(data: SimData) -> SimData: body_rate_ctrl, mask, r_int_error=r_int_error, - last_ang_vel=states.ang_vel, + prev_ang_vel=states.ang_vel, steps=data.core.steps, ) ft_ctrl = leaf_replace( @@ -746,12 +736,7 @@ def control_commit_attitude(data: SimData) -> SimData: """Commit the staged attitude command to the controller setpoint.""" attitude_ctrl: MellingerAttitudeData = data.controls.attitude mask = controllable(data.core.steps, data.core.freq, attitude_ctrl.steps, attitude_ctrl.freq) - attitude_ctrl = leaf_replace( - attitude_ctrl, - mask, - cmd=attitude_ctrl.staged_cmd, - ang_vel_des=attitude_ctrl.staged_ang_vel_des, - ) + attitude_ctrl = leaf_replace(attitude_ctrl, mask, cmd=attitude_ctrl.staged_cmd) return data.replace(controls=data.controls.replace(attitude=attitude_ctrl)) diff --git a/docs/user-guide/control/mellinger.md b/docs/user-guide/control/mellinger.md index fd62fd5f..4df88988 100644 --- a/docs/user-guide/control/mellinger.md +++ b/docs/user-guide/control/mellinger.md @@ -24,7 +24,7 @@ All three stages share the same state convention: | `pos` | `(..., 3)` | Current position [m] | | `quat` | `(..., 4)` | Current attitude, xyzw | | `vel` | `(..., 3)` | Current velocity [m/s] | -| `cmd` | `(..., 16)` | Setpoint: `[x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, wx, wy, wz]`. Only the yaw of the quaternion `qx, qy, qz, qw` is used, as in the firmware. The body rates `wx, wy, wz` are not used by this stage, see below | +| `cmd` | `(..., 16)` | Setpoint: `[x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, wx, wy, wz]` | | `pos_err_i` | `(..., 3)` or `None` | Position integral error from the previous call. `None` initialises to zero | | `ctrl_freq` | `float` | Control frequency in Hz (default 100) | @@ -35,7 +35,7 @@ All three stages share the same state convention: | `rpyt` | `(..., 4)` | Attitude + thrust: `[roll_rad, pitch_rad, yaw_rad, thrust_N]` | | `pos_err_i` | `(..., 3)` | Position integral error. Pass back as `pos_err_i` on the next call | -As in the firmware, the position controller does not use the body rates of the setpoint. They are the body rate setpoint of the attitude controller. `Control.state` forwards them to the attitude stage automatically. `attitude2force_torque` assumes a zero body rate setpoint. +As in the firmware, only the yaw of the quaternion `qx, qy, qz, qw` is used. The body rates are forwarded to the attitude stage automatically. ```python import numpy as np diff --git a/docs/user-guide/functional-api.md b/docs/user-guide/functional-api.md index de7a88fb..52114f1d 100644 --- a/docs/user-guide/functional-api.md +++ b/docs/user-guide/functional-api.md @@ -58,7 +58,7 @@ From this point, `data` is a plain JAX pytree and `step` and `reset` are compile ## Purely functional controller functions -`crazyflow.sim.functional` mirrors all five `Sim` control methods as pure functions: +`crazyflow.sim.functional` mirrors all `Sim` control methods as pure functions: ```python import crazyflow.sim.functional as F diff --git a/docs/user-guide/oo-api.md b/docs/user-guide/oo-api.md index 0bcfe1ea..441022d6 100644 --- a/docs/user-guide/oo-api.md +++ b/docs/user-guide/oo-api.md @@ -50,7 +50,7 @@ All control methods take an array of shape `(n_worlds, n_drones, command_dim)` a ### State control -The highest-level interface. A 16-element command sets desired position, velocity, acceleration, attitude, and body rates. Only the yaw of the attitude quaternion is used, as in the firmware. An internal Mellinger controller converts this to attitude commands and forwards the body rates to the attitude controller. +The highest-level interface. A 16-element command sets desired position, velocity, acceleration, attitude, and body rates. The yaw part of the attitude commands the heading. The body rate setpoint is forwarded to the attitude controller. ```python import numpy as np diff --git a/tests/unit/test_functional.py b/tests/unit/test_functional.py index 9594a60d..b780a3f5 100644 --- a/tests/unit/test_functional.py +++ b/tests/unit/test_functional.py @@ -130,21 +130,21 @@ def test_functional_state_control(state_freq: int): assert jnp.all(controllable[1] == can_control_2[i]), f"Controllable 2 mismatch at t={i}" # Apply control data = F.state_control(data, cmd) - last_attitude = data.controls.attitude.staged_cmd + prev_attitude = data.controls.attitude.staged_cmd data = step_fn(data, 1) attitude = data.controls.attitude.staged_cmd - last_att, att = last_attitude[0], attitude[0] + prev_att, att = prev_attitude[0], attitude[0] if can_control_1[i]: - assert not jnp.all(att == last_att), f"Controls haven't been applied at t={i}" + assert not jnp.all(att == prev_att), f"Controls haven't been applied at t={i}" else: - assert jnp.all(att == last_att), f"Controls should be unchanged at t={i}" + assert jnp.all(att == prev_att), f"Controls should be unchanged at t={i}" - last_att, att = last_attitude[1], attitude[1] + prev_att, att = prev_attitude[1], attitude[1] if can_control_2[i]: - assert not jnp.all(att == last_att), f"Controls haven't been applied at t={i}" + assert not jnp.all(att == prev_att), f"Controls haven't been applied at t={i}" else: - assert jnp.all(att == last_att), f"Controls should be unchanged at t={i}" + assert jnp.all(att == prev_att), f"Controls should be unchanged at t={i}" if i == 0: # Make world 2 asynchronous data = reset_fn(data, default_data, np.array([False, True])) diff --git a/tests/unit/test_sim.py b/tests/unit/test_sim.py index 3bf0bae9..088e1fa1 100644 --- a/tests/unit/test_sim.py +++ b/tests/unit/test_sim.py @@ -293,19 +293,19 @@ def test_sim_state_control(state_freq: int): assert jnp.all(sim.controllable[0] == can_control_1[i]), f"Controllable 1 mismatch at t={i}" assert jnp.all(sim.controllable[1] == can_control_2[i]), f"Controllable 2 mismatch at t={i}" sim.state_control(cmd) - last_attitude = sim.data.controls.attitude.staged_cmd + prev_attitude = sim.data.controls.attitude.staged_cmd sim.step() attitude = sim.data.controls.attitude.staged_cmd - last_att, att = last_attitude[0], attitude[0] + prev_att, att = prev_attitude[0], attitude[0] if can_control_1[i]: - assert not jnp.all(att == last_att), f"Controls haven't been applied at t={i}" + assert not jnp.all(att == prev_att), f"Controls haven't been applied at t={i}" else: - assert jnp.all(att == last_att), f"Controls should be unchanged at t={i}" - last_att, att = last_attitude[1], attitude[1] + assert jnp.all(att == prev_att), f"Controls should be unchanged at t={i}" + prev_att, att = prev_attitude[1], attitude[1] if can_control_2[i]: - assert not jnp.all(att == last_att), f"Controls haven't been applied at t={i}" + assert not jnp.all(att == prev_att), f"Controls haven't been applied at t={i}" else: - assert jnp.all(att == last_att), f"Controls should be unchanged at t={i}" + assert jnp.all(att == prev_att), f"Controls should be unchanged at t={i}" if i == 0: sim.reset(np.array([False, True])) # Make world 2 asynchronous From 1400489d62100ed62a5491aa691719ee8ced0566 Mon Sep 17 00:00:00 2001 From: Martin Schuck Date: Mon, 14 Sep 2026 23:39:59 +0200 Subject: [PATCH 6/6] Review comments --- crazyflow/control/mellinger/control.py | 3 +-- docs/user-guide/control/controllers.md | 2 +- docs/user-guide/control/index.md | 6 +++--- docs/user-guide/control/mellinger.md | 2 +- docs/user-guide/sim-overview.md | 1 - 5 files changed, 6 insertions(+), 8 deletions(-) diff --git a/crazyflow/control/mellinger/control.py b/crazyflow/control/mellinger/control.py index faa1b142..6e05d7b4 100644 --- a/crazyflow/control/mellinger/control.py +++ b/crazyflow/control/mellinger/control.py @@ -492,8 +492,7 @@ class MellingerStateData: cmd: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 16) """Full state control command for the drone. - A command consists of [x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, wx, wy, wz]. Only the - yaw of the attitude quaternion is used. The body rates are forwarded to the attitude controller. + A command consists of [x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, wx, wy, wz]. """ staged_cmd: Array = field(metadata={CORE_NDIM_KEY: 1}) # (N, M, 16) """Staging buffer to store the most recent command until the next controller tick.""" diff --git a/docs/user-guide/control/controllers.md b/docs/user-guide/control/controllers.md index da2d8f5a..66f28aa2 100644 --- a/docs/user-guide/control/controllers.md +++ b/docs/user-guide/control/controllers.md @@ -16,7 +16,7 @@ The Mellinger controller [[1]](#references) is split into three stages that form | 2 | [`attitude2force_torque`](mellinger.md#attitude-to-force-torque) | Attitude + RPYT command | Collective force, body torques + angular velocity integral error | | 3 | [`force_torque2rotor_vel`](mellinger.md#force-torque-to-rotor-velocities) | Force + torques | 4 motor speeds [RPM] | -[`body_rate2force_torque`](mellinger.md#body-rate-to-force-torque) replaces stage 2 when the command is a body rate setpoint instead of an attitude. It runs the same controller with the rate setpoint in the angular velocity error and a level attitude setpoint, as the firmware does. +[`body_rate2force_torque`](mellinger.md#body-rate-to-force-torque) replaces stage 2 when the command is a body rate setpoint instead of an attitude. It runs the same controller with the rate setpoint in the angular velocity error and a level attitude setpoint. ## Available controllers diff --git a/docs/user-guide/control/index.md b/docs/user-guide/control/index.md index 75adeecf..12d5e597 100644 --- a/docs/user-guide/control/index.md +++ b/docs/user-guide/control/index.md @@ -1,10 +1,10 @@ # Control Modes -Crazyflow provides five control modes, from high-level position setpoints down to direct motor commands. Each mode is selected at construction time. +Crazyflow provides multiple control modes, from high-level position setpoints down to direct motor commands. Each mode is selected at construction time. ## Control hierarchy -Commands flow down a hierarchy. A state command is converted to an attitude command by the Mellinger position controller; the attitude command is converted to force/torque by the geometric controller; force/torque is converted to rotor velocities by the mixer. Body rate control feeds the geometric controller with a rate setpoint instead of an attitude, so it enters the hierarchy at the same level as attitude control. The rate setpoint in the state command is forwarded to the rate controller. +Commands flow down a hierarchy. A state command is converted to an attitude command by the Mellinger position controller, the attitude command is converted to force/torque by the geometric controller, and force/torque is converted to rotor velocities by the mixer. Body rate control feeds the geometric controller with a rate setpoint instead of an attitude, so it enters the hierarchy at the same level as attitude control. The rate setpoint in the state command is forwarded to the rate controller. ``` State (16D) @@ -38,7 +38,7 @@ Command shape: `(n_worlds, n_drones, 16)` | 9–12 | Attitude quaternion \(q_x, q_y, q_z, q_w\) | | | 13–15 | Body rates \(\omega_x, \omega_y, \omega_z\) | rad/s | -As in the firmware's full state setpoint, only the yaw of the attitude quaternion is used. The body rates are the angular velocity in the body frame. The position controller does not use them and forwards them to the attitude controller as its body rate setpoint. The so_rpy family ignores the body rate setpoint. +As in the firmware's full state setpoint, only the yaw of the attitude quaternion is used. The body rates are the angular velocity in the body frame. The so_rpy family ignores them. Set unused elements to zero. The attitude quaternion must be valid. A common hover command sets only the z position: diff --git a/docs/user-guide/control/mellinger.md b/docs/user-guide/control/mellinger.md index 4df88988..0c61126b 100644 --- a/docs/user-guide/control/mellinger.md +++ b/docs/user-guide/control/mellinger.md @@ -35,7 +35,7 @@ All three stages share the same state convention: | `rpyt` | `(..., 4)` | Attitude + thrust: `[roll_rad, pitch_rad, yaw_rad, thrust_N]` | | `pos_err_i` | `(..., 3)` | Position integral error. Pass back as `pos_err_i` on the next call | -As in the firmware, only the yaw of the quaternion `qx, qy, qz, qw` is used. The body rates are forwarded to the attitude stage automatically. +As in the firmware, only the yaw of the quaternion `qx, qy, qz, qw` is used. ```python import numpy as np diff --git a/docs/user-guide/sim-overview.md b/docs/user-guide/sim-overview.md index a39fa03d..5100ac74 100644 --- a/docs/user-guide/sim-overview.md +++ b/docs/user-guide/sim-overview.md @@ -75,7 +75,6 @@ from crazyflow.control import Control from scipy.spatial.transform import Rotation as R sim = Sim(freq=500, control=Control.state) -sim.reset() cmd = np.zeros((1, 1, 16), dtype=np.float32) cmd[..., 9:13] = R.from_euler("z", 0.0).as_quat() sim.state_control(cmd)