Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 2 additions & 14 deletions crazyflow/control/mellinger/control.py
Original file line number Diff line number Diff line change
Expand Up @@ -162,7 +162,6 @@ def attitude2force_torque(
int_err_max: Array,
torque_pwm_max: Array,
thrust_max: float,
pwm_min: float,
pwm_max: float,
L: float,
thrust2torque: float,
Expand Down Expand Up @@ -190,7 +189,6 @@ def attitude2force_torque(
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).
Expand Down Expand Up @@ -218,7 +216,6 @@ def attitude2force_torque(
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,
Expand All @@ -242,7 +239,6 @@ def body_rate2force_torque(
int_err_max: Array,
torque_pwm_max: Array,
thrust_max: float,
pwm_min: float,
pwm_max: float,
L: float,
thrust2torque: float,
Expand Down Expand Up @@ -278,7 +274,6 @@ def body_rate2force_torque(
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).
Expand Down Expand Up @@ -311,7 +306,6 @@ def body_rate2force_torque(
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,
Expand All @@ -337,7 +331,6 @@ def _attitude2force_torque(
int_err_max: Array,
torque_pwm_max: Array,
thrust_max: float,
pwm_min: float,
pwm_max: float,
L: float,
thrust2torque: float,
Expand Down Expand Up @@ -367,7 +360,6 @@ def _attitude2force_torque(
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).
Expand Down Expand Up @@ -412,11 +404,6 @@ def _attitude2force_torque(
rot_err_i = xp.where((force_des > 0)[..., None], rot_err_i, 0.0)
force_des_pwm = force2pwm(force_des / 4, thrust_max, pwm_max)
pwms = force_torque_pwms2pwms(force_des_pwm, torque_pwm, mixing_matrix)
# We assume battery compensation is perfect. Motors below pwm_min are turned off. See
# https://github.com/bitcraze/crazyflie-firmware/blob/d794c5ca081d19aa1e1bbcb6fd4a3711784fb8b1/src/drivers/src/motors.c#L199-L202
pwms = xp.where(pwms < pwm_min, 0.0, pwms)
excess = xp.clip(xp.max(pwms, axis=-1, keepdims=True) - pwm_max, min=0.0)
pwms = xp.clip(pwms - excess, min=0.0)

# Info: The Mellinger controller in the firmware ends here. However, we enforce a standardized
# interface in the simulation from states -> attitude -> force_torque. We therefore need this
Expand Down Expand Up @@ -486,7 +473,8 @@ def force_torque2rotor_vel(
assert force.shape[-1] == 1, f"Force must have shape (..., 1), but has {force.shape}"
torque_forces = (torque * xp.asarray([1 / L, 1 / L, 1 / thrust2torque])) @ mixing_matrix
motor_forces = (torque_forces + force) / 4
# Clip motor forces on the thrust instead of PWM level.
# Clip motor forces on the thrust instead of PWM level. Motors below thrust_min are turned off,
# see https://github.com/bitcraze/crazyflie-firmware/blob/d794c5ca081d19aa1e1bbcb6fd4a3711784fb8b1/src/drivers/src/motors.c#L199-L202
motor_forces = xp.where(motor_forces < thrust_min, 0.0, motor_forces)
excess = xp.clip(xp.max(motor_forces, axis=-1, keepdims=True) - thrust_max, min=0.0)
motor_forces = xp.clip(motor_forces - excess, min=0.0)
Expand Down
4 changes: 2 additions & 2 deletions crazyflow/sim/sim.py
Original file line number Diff line number Diff line change
Expand Up @@ -827,8 +827,8 @@ def rotor_vel_limits(dynamics: Dynamics, drone: Drone) -> tuple[float, float]:
params = load_dynamics_params(dynamics, drone)
thrust_min, thrust_max = float(params["thrust_min"]), float(params["thrust_max"])
if dynamics == Dynamics.first_principles:
rpm = motor_force2rotor_vel(np.asarray([thrust_min, thrust_max]), params["rpm2thrust"])
return float(rpm[0]), float(rpm[1])
rpm_max = motor_force2rotor_vel(np.asarray(thrust_max), params["rpm2thrust"])
return 0.0, float(rpm_max)
return 4 * thrust_min, 4 * thrust_max


Expand Down
53 changes: 36 additions & 17 deletions tests/fuzzing/test_mellinger.py
Original file line number Diff line number Diff line change
Expand Up @@ -15,10 +15,11 @@
from hypothesis.extra import numpy as hnp
from scipy.spatial.transform import Rotation as R

from crazyflow.control import load_fn_params, load_params, parametrize
from crazyflow.control import Control, load_fn_params, load_params, parametrize
from crazyflow.control.mellinger import control
from crazyflow.control.transform import force2pwm, motor_force2rotor_vel, pwm2force
from crazyflow.drones import Drone
from crazyflow.sim import Sim

if TYPE_CHECKING:
from numpy.typing import NDArray
Expand Down Expand Up @@ -240,8 +241,9 @@ def test_body_rate2force_torque(
np.testing.assert_allclose(thrust, out.thrust, atol=1)
# See test_attitude2force_torque for why we assume a minimum thrust for torque comparison
assume(abs(out.thrust) > 1)
# The yaw is undefined at 90 degrees pitch, and both controllers level at the current yaw
assume(abs(R.from_quat(quat).as_matrix()[2, 0]) < 1 - 1e-6)
# The yaw is undefined at 90 degrees pitch
pitch = R.from_quat(quat).as_euler("xyz", suppress_warnings=True)[1]
assume(abs(pitch) < np.pi / 2 - 1e-3)
np.testing.assert_allclose(torque, [out.roll, out.pitch, out.yaw], atol=2)
ref_rot_err_i = [ref.i_error_m_x, -ref.i_error_m_y, ref.i_error_m_z]
np.testing.assert_allclose(rot_err_i, ref_rot_err_i, atol=1e-6)
Expand All @@ -264,24 +266,41 @@ def test_mixer(thrust: NDArray, torque: NDArray):
np.testing.assert_allclose(motors, [m.m1, m.m2, m.m3, m.m4], atol=3)


@pytest.mark.xfail(strict=True, reason="TODO: Use the current and previous body rate setpoint")
@pytest.fixture(scope="module")
def sim() -> Sim:
"""Simulate one cf2x_L250 with the state controller at the firmware's controller rate."""
sim = Sim(drone=Drone.cf2x_L250, control=Control.state, state_freq=cf.ATTITUDE_RATE)
sim.step() # Compile outside the examples to stay within the hypothesis deadline
return sim


@given(state=_states(), ctrl_state=_ctrl_states(), cmd=_state_cmds())
def test_end_to_end(state: tuple[NDArray, ...], ctrl_state: tuple[NDArray, ...], cmd: NDArray):
"""Compare the rotor velocities for state commands."""
pos, vel, quat, ang_vel = map(jnp.asarray, state)
pos_err_i, rot_err_i, prev_ang_vel, _ = map(jnp.asarray, ctrl_state)
# Jitting the module functions keeps JAX's compilation cache across examples
state2attitude = parametrize(jax.jit(control.state2attitude), Drone.cf2x_L250, xp=jnp)
attitude2force_torque = parametrize(
jax.jit(control.attitude2force_torque), Drone.cf2x_L250, xp=jnp
def test_end_to_end(
sim: Sim, state: tuple[NDArray, ...], ctrl_state: tuple[NDArray, ...], cmd: NDArray
):
"""Compare the rotor velocities of a simulated state control step."""
# The setpoint yaw is undefined at 90 degrees pitch
pitch_des = R.from_quat(cmd[9:13]).as_euler("xyz", suppress_warnings=True)[1]
assume(abs(pitch_des) < np.pi / 2 - 1e-3)
pos, vel, quat, ang_vel = jax.device_put([x[None, None] for x in state], sim.device)
pos_err_i, rot_err_i, prev_ang_vel, prev_ang_vel_des = jax.device_put(
[x[None, None] for x in ctrl_state], sim.device
)
force_torque2rotor_vel = parametrize(
jax.jit(control.force_torque2rotor_vel), Drone.cf2x_L250, xp=jnp
sim.reset()
data = sim.data
controls = sim.data.controls
data = data.replace(states=data.states.replace(pos=pos, vel=vel, quat=quat, ang_vel=ang_vel))
controls = controls.replace(state=data.controls.state.replace(pos_err_i=pos_err_i))
controls = controls.replace(
attitude=data.controls.attitude.replace(
rot_err_i=rot_err_i, prev_ang_vel=prev_ang_vel, ang_vel_des=prev_ang_vel_des
)
)
sim.data = data.replace(controls=controls)
sim.state_control(jax.device_put(cmd[None, None], sim.device))
sim.step()
rotor_vel = sim.data.controls.rotor_vel[0, 0]
params = load_params("mellinger", Drone.cf2x_L250)["core"]
rpyt, _ = state2attitude(pos, quat, vel, jnp.asarray(cmd), pos_err_i, cf.ATTITUDE_RATE)
force, torque, _ = attitude2force_torque(quat, ang_vel, rpyt, prev_ang_vel, rot_err_i)
rotor_vel = force_torque2rotor_vel(force, torque)
setpoint = cf.setpoint_t()
sp_pos = setpoint.position
sp_pos.x, sp_pos.y, sp_pos.z = cmd[:3].tolist()
Expand Down
2 changes: 1 addition & 1 deletion tests/unit/test_sim.py
Original file line number Diff line number Diff line change
Expand Up @@ -466,7 +466,7 @@ def test_rotor_vel_clip(integrator: Integrator):
device="cpu",
)
lower, upper = rotor_vel_limits(Dynamics.first_principles, sim.drone)
assert 0.0 < lower < upper
assert 0.0 < upper

for value, target in ((2 * upper, upper), (-upper, lower)):
sim.reset()
Expand Down