From e833afa8a1b11f3bbdb968d912ce324bdfc83833 Mon Sep 17 00:00:00 2001 From: ratheron Date: Tue, 6 Oct 2026 20:13:17 +0200 Subject: [PATCH 1/4] Fix dynamics equations to match the paper and implementation Fixes #130 --- .../dynamics/first_principles/__init__.py | 86 ++++++++++++------- .../dynamics/first_principles/dynamics.py | 9 +- .../dynamics/first_principles/params.toml | 2 +- crazyflow/dynamics/so_rpy/__init__.py | 27 +++--- crazyflow/dynamics/so_rpy_rotor/__init__.py | 36 +++++--- .../dynamics/so_rpy_rotor_drag/__init__.py | 29 ++++--- 6 files changed, 116 insertions(+), 73 deletions(-) diff --git a/crazyflow/dynamics/first_principles/__init__.py b/crazyflow/dynamics/first_principles/__init__.py index 62076062..57072dbe 100644 --- a/crazyflow/dynamics/first_principles/__init__.py +++ b/crazyflow/dynamics/first_principles/__init__.py @@ -1,30 +1,39 @@ r"""Full rigid-body dynamics for a quadrotor. -This package implements Newton-Euler dynamics based on physical constants: mass, inertia, motor +This package implements Newton-Euler dynamics based on physical parameters: mass, inertia, motor thrust and torque curves, arm length, and drag coefficients. The command interface is four motor -angular velocities in RPM. No data fitting is required; all parameters are measurable physical -quantities. +angular velocities in RPM. Mass and arm length are measured directly and the propeller inertia is +taken from CAD data. The thrust and torque curves are fitted to load cell data, and the inertia and +drag coefficients are identified from flight data. Motor forces and torques are quadratic polynomials in RPM: \[ - f_{p,i} = k_0 + k_1 \Omega_i + k_2 \Omega_i^2, \qquad - \tau_{p,i} = m_0 + m_1 \Omega_i + m_2 \Omega_i^2. + f_{\mathrm{m},i} = k_0 + k_1 \Omega_i + k_2 \Omega_i^2, \qquad + t_{\mathrm{m},i} = m_0 + m_1 \Omega_i + m_2 \Omega_i^2, \] +where \(\Omega_i\) is the rotor speed of motor \(i\) in RPM, \(f_{\mathrm{m},i}\) and +\(t_{\mathrm{m},i}\) are its thrust and drag torque, and \(k_0, k_1, k_2\) and \(m_0, m_1, m_2\) are +the thrust and torque curve coefficients. + When rotor dynamics are modelled, each motor RPM evolves as: \[ \dot{\Omega}_i = \begin{cases} - c_1 (\Omega_{\mathrm{cmd},i} - \Omega_i) - + c_2 (\Omega_{\mathrm{cmd},i}^2 - \Omega_i^2) + \hat{c}_\mathrm{v} (\Omega_{\mathrm{cmd},i} - \Omega_i) + + \hat{c}_\mathrm{d} (\Omega_{\mathrm{cmd},i}^2 - \Omega_i^2) & \Omega_{\mathrm{cmd},i} \geq \Omega_i \\[4pt] - c_3 (\Omega_{\mathrm{cmd},i} - \Omega_i) - + c_4 (\Omega_{\mathrm{cmd},i}^2 - \Omega_i^2) + \check{c}_\mathrm{v} (\Omega_{\mathrm{cmd},i} - \Omega_i) + + \check{c}_\mathrm{d} (\Omega_{\mathrm{cmd},i}^2 - \Omega_i^2) & \Omega_{\mathrm{cmd},i} < \Omega_i \end{cases} \] +where \(\Omega_{\mathrm{cmd},i}\) is the commanded rotor speed, \(\hat{c}_\mathrm{v}\) and +\(\hat{c}_\mathrm{d}\) are the viscous damping and drag parameters during acceleration, and +\(\check{c}_\mathrm{v}\) and \(\check{c}_\mathrm{d}\) those during deceleration. + The rigid-body equations of motion are: \[ @@ -33,8 +42,8 @@ \dot{\mathbf{q}} &= \tfrac{1}{2} \mathbf{q} \otimes \begin{bmatrix} {}^{\mathcal{B}}\boldsymbol{\omega}\\0 \end{bmatrix}, \\ m\dot{\mathbf{v}} &= m\mathbf{g} - + R\,{}^{\mathcal{B}}\mathbf{f}_t - + R\,{}^{\mathcal{B}}\mathbf{f}_a, \\ + + \mathbf{R}\,{}^{\mathcal{B}}\mathbf{f}_\mathrm{t} + + \mathbf{R}\,{}^{\mathcal{B}}\mathbf{f}_\mathrm{a}, \\ \mathbf{J}\,{}^{\mathcal{B}}\dot{\boldsymbol{\omega}} &= {}^{\mathcal{B}}\mathbf{t}_\Sigma - {}^{\mathcal{B}}\boldsymbol{\omega} @@ -42,18 +51,23 @@ \end{aligned} \] -where \(R = {}^{\mathcal{I}}R_{\mathcal{B}}(\mathbf{q})\) is the rotation from body to world frame, -and the forces and torques are: +where \(\mathbf{p}\) and \(\mathbf{v}\) are the position and velocity, \(\mathbf{q}\) is the +orientation as a scalar-last quaternion, \({}^{\mathcal{B}}\boldsymbol{\omega}\) is the angular +velocity, \({}^{\mathcal{B}}(\cdot)\) denotes the body frame, \(\otimes\) is the quaternion product, +\(m\) is the mass, \(\mathbf{J}\) is the inertia matrix, \(\mathbf{g}\) is the gravity vector, +\(\mathbf{R} = {}^{\mathcal{I}}\mathbf{R}_{\mathcal{B}}(\mathbf{q})\) is the rotation from body to +world frame, and the forces and torques are: \[ \begin{aligned} - {}^{\mathcal{B}}\mathbf{f}_t &= - \mathbf{e}_z \textstyle\sum_{i=1}^{4} f_{p,i}, \\ - {}^{\mathcal{B}}\mathbf{f}_a &= D_b\,R^{\top}\mathbf{v}, \\ + {}^{\mathcal{B}}\mathbf{f}_\mathrm{t} &= + \mathbf{e}_\mathrm{z} \textstyle\sum_{i=1}^{4} f_{\mathrm{m},i}, \\ + {}^{\mathcal{B}}\mathbf{f}_\mathrm{a} &= \mathbf{C}_\mathrm{a}\,\mathbf{R}^{\top}\mathbf{v}, \\ {}^{\mathcal{B}}\mathbf{t}_\Sigma &= - {}^{\mathcal{B}}\mathbf{t}_t - + {}^{\mathcal{B}}\mathbf{t}_d - + {}^{\mathcal{B}}\mathbf{t}_i, + {}^{\mathcal{B}}\mathbf{t}_\mathrm{t} + + {}^{\mathcal{B}}\mathbf{t}_\mathrm{d} + + {}^{\mathcal{B}}\mathbf{t}_\mathrm{g} + + {}^{\mathcal{B}}\mathbf{t}_\mathrm{r}, \end{aligned} \] @@ -61,24 +75,32 @@ \[ \begin{aligned} - {}^{\mathcal{B}}\mathbf{t}_t &= - \frac{l}{\sqrt{2}} + {}^{\mathcal{B}}\mathbf{t}_\mathrm{t} &= + l \begin{bmatrix}1&0&0\\0&1&0\\0&0&0\end{bmatrix} - M\,\mathbf{f}_p, \\ - {}^{\mathcal{B}}\mathbf{t}_d &= + \mathbf{M}\,\mathbf{f}_\mathrm{m}, \\ + {}^{\mathcal{B}}\mathbf{t}_\mathrm{d} &= \begin{bmatrix}0&0&0\\0&0&0\\0&0&1\end{bmatrix} - M\,\boldsymbol{\tau}_p, \\ - {}^{\mathcal{B}}\mathbf{t}_i &= J_p - \begin{bmatrix} - -{}^{\mathcal{B}}\omega_y\;\mathbf{m}_z^{\top}\boldsymbol{\Omega} \\ - -{}^{\mathcal{B}}\omega_x\;\mathbf{m}_z^{\top}\boldsymbol{\Omega} \\ - \mathbf{m}_z^{\top}\dot{\boldsymbol{\Omega}} - \end{bmatrix}, + \mathbf{M}\,\mathbf{t}_\mathrm{m}, \\ + {}^{\mathcal{B}}\mathbf{t}_\mathrm{g} &= J_\mathrm{p} + \left({}^{\mathcal{B}}\boldsymbol{\omega} \times \mathbf{e}_\mathrm{z}\right) + \mathbf{e}_\mathrm{z}^{\top} \mathbf{M}\,\boldsymbol{\Omega}, \\ + {}^{\mathcal{B}}\mathbf{t}_\mathrm{r} &= J_\mathrm{p}\,\mathbf{e}_\mathrm{z}\, + \mathbf{e}_\mathrm{z}^{\top} \mathbf{M}\,\dot{\boldsymbol{\Omega}}, \end{aligned} \] -where \(D_b\) is the body-frame drag matrix, \(l\) is the motor arm length, \(J_p\) is the propeller -moment of inertia, \(M\) is the \(3\times 4\) mixing matrix, and \(\mathbf{m}_z\) is its last row. +where \({}^{\mathcal{B}}\mathbf{f}_\mathrm{t}\) and \({}^{\mathcal{B}}\mathbf{f}_\mathrm{a}\) are +the thrust and aerodynamic drag force, \({}^{\mathcal{B}}\mathbf{t}_\mathrm{t}\), +\({}^{\mathcal{B}}\mathbf{t}_\mathrm{d}\), \({}^{\mathcal{B}}\mathbf{t}_\mathrm{g}\), and +\({}^{\mathcal{B}}\mathbf{t}_\mathrm{r}\) are the thrust, drag, gyroscopic, and reaction torque, +\(\mathbf{e}_\mathrm{z}\) is the unit vector in z direction, \(\mathbf{C}_\mathrm{a}\) is the +body-frame drag matrix, \(l\) is the distance of the motors to the body axes, \(\mathbf{M}\) is +the \(3\times 4\) mixing matrix, \(J_\mathrm{p}\) is the combined inertia of one propeller and its +motor, and \(\mathbf{f}_\mathrm{m}\), \(\mathbf{t}_\mathrm{m}\), and \(\boldsymbol{\Omega}\) stack +the four motor thrusts, torques, and rotor speeds. The gyroscopic and reaction torques convert +\(\boldsymbol{\Omega}\) and \(\dot{\boldsymbol{\Omega}}\) to rad/s internally, so \(J_\mathrm{p}\) +is in SI units. """ from crazyflow.dynamics.first_principles.dynamics import ( diff --git a/crazyflow/dynamics/first_principles/dynamics.py b/crazyflow/dynamics/first_principles/dynamics.py index 38b85f22..62824fa1 100644 --- a/crazyflow/dynamics/first_principles/dynamics.py +++ b/crazyflow/dynamics/first_principles/dynamics.py @@ -1,9 +1,8 @@ """First-principles dynamics-based quadrotor dynamics. This module implements full rigid-body dynamics for a quadrotor based on Newton-Euler equations. The -dynamics are parameterised with physical constants (mass, inertia, thrust and torque curves, motor -arm length, drag coefficients) and require no data fitting. Propeller gyroscopic effects are -included. +dynamics are parameterised with physical quantities (mass, inertia, thrust and torque curves, motor +arm length, drag coefficients). Propeller gyroscopic effects are included. The command interface is four motor angular velocities in RPM. @@ -79,7 +78,7 @@ def dynamics( dist_t: Disturbance torque (Nm) in the world frame acting on the CoM. mass: Mass of the drone (kg). - L: Distance from the CoM to the motors (m). Shared (1,) or one value per motor (4,). + L: Distance of the motors to the body axes (m). Shared (1,) or one value per motor (4,). prop_inertia: Inertia of the propellers in z direction (kg m^2). Shared (1,) or one value per motor (4,). gravity_vec: Gravity vector (m/s^2). We assume the gravity vector points downwards, e.g. @@ -205,7 +204,7 @@ def symbolic_dynamics( model_dist_f: If ``True``, a 3-D force disturbance is appended to ``X``. model_dist_t: If ``True``, a 3-D torque disturbance is appended to ``X``. mass: Drone mass in kg. - L: Distance from centre of mass to the motors in meters, shared ``(1,)`` or one value per + L: Distance of the motors to the body axes in meters, shared ``(1,)`` or one value per motor ``(4,)``. prop_inertia: Moment of inertia of the propellers about their spin axis in kg m², shared ``(1,)`` or one value per motor ``(4,)``. diff --git a/crazyflow/dynamics/first_principles/params.toml b/crazyflow/dynamics/first_principles/params.toml index 0d2fda1b..c185599e 100644 --- a/crazyflow/dynamics/first_principles/params.toml +++ b/crazyflow/dynamics/first_principles/params.toml @@ -9,7 +9,7 @@ # ] # thrust_min = 0.0 # N per motor # thrust_max = 0.0 # N per motor -# L = 0.0 # m, CoM to motor distance +# L = 0.0 # m, distance of the motors to the body axes # prop_inertia = 0.0 # kg m^2, zero drops the gyroscopic and reaction torque of the propellers # rpm2thrust = [0.0, 0.0, 0.0] # N, polynomial in RPM by index # rpm2torque = [0.0, 0.0, 0.0] # Nm, polynomial in RPM by index diff --git a/crazyflow/dynamics/so_rpy/__init__.py b/crazyflow/dynamics/so_rpy/__init__.py index c71d4324..3399e661 100644 --- a/crazyflow/dynamics/so_rpy/__init__.py +++ b/crazyflow/dynamics/so_rpy/__init__.py @@ -8,26 +8,33 @@ \begin{aligned} \dot{\mathbf{p}} &= \mathbf{v}, \\ m\dot{\mathbf{v}} &= m\mathbf{g} - + (c_{\mathrm{acc}} + c_f F_{\mathrm{cmd}})\,R\,\mathbf{e}_z, \\ - \ddot{\boldsymbol{\psi}} &= - c_{\psi}\,\boldsymbol{\psi} - + c_{\dot{\psi}}\,\dot{\boldsymbol{\psi}} - + c_u\,\mathbf{u}_{\mathrm{rpy}}, + + (c_\mathrm{acc} + c_\mathrm{f} f_{\Sigma,\mathrm{cmd}}) + \,\mathbf{R}\,\mathbf{e}_\mathrm{z}, \\ + \ddot{\boldsymbol{\Psi}} &= + \boldsymbol{c}_{\boldsymbol{\Psi},1}\,\boldsymbol{\Psi} + + \boldsymbol{c}_{\boldsymbol{\Psi},2}\,\dot{\boldsymbol{\Psi}} + + \boldsymbol{c}_{\boldsymbol{\Psi},3}\,\boldsymbol{\Psi}_\mathrm{cmd}, \end{aligned} \] -The vector \(\boldsymbol{\psi} = [\phi,\theta,\psi]^{\top}\) holds the roll, pitch, and yaw angles -with rates \(\dot{\boldsymbol{\psi}}\). The coefficients \(c_{\psi}\), \(c_{\dot{\psi}}\), and -\(c_u\) are identified from flight data. +where \(\mathbf{p}\) and \(\mathbf{v}\) are the position and velocity, \(m\) is the mass, +\(\mathbf{g}\) is the gravity vector, \(\mathbf{e}_\mathrm{z}\) is the unit vector in z direction, +\(\mathbf{R} = {}^{\mathcal{I}}\mathbf{R}_{\mathcal{B}}(\boldsymbol{\Psi})\) is the rotation from +body to world frame, \(\boldsymbol{\Psi} = [\phi,\theta,\psi]^{\top}\) holds the roll, pitch, and +yaw angles with rates \(\dot{\boldsymbol{\Psi}}\), and \(f_{\Sigma,\mathrm{cmd}}\) and +\(\boldsymbol{\Psi}_\mathrm{cmd}\) are the commanded collective thrust and attitude. The thrust +offset \(c_\mathrm{acc}\), the thrust scaling coefficient \(c_\mathrm{f}\), and the rotational +coefficients \(\boldsymbol{c}_{\boldsymbol{\Psi},1}\), \(\boldsymbol{c}_{\boldsymbol{\Psi},2}\), and +\(\boldsymbol{c}_{\boldsymbol{\Psi},3}\) are identified from flight data. !!! note This is the native Euler-angle form, matching [symbolic_dynamics_euler][crazyflow.dynamics.so_rpy.symbolic_dynamics_euler]. The simulation does not integrate this state directly. It shares the common ``[pos, quat, vel, ang_vel]`` state with the other models and advances the orientation from the body angular velocity - \({}^{\mathcal{B}}\boldsymbol{\omega}\), converting \(\ddot{\boldsymbol{\psi}} \leftrightarrow + \({}^{\mathcal{B}}\boldsymbol{\omega}\), converting \(\ddot{\boldsymbol{\Psi}} \leftrightarrow {}^{\mathcal{B}}\dot{\boldsymbol{\omega}}\) through the kinematic Jacobian at every step. - Integrating from \({}^{\mathcal{B}}\boldsymbol{\omega}\) rather than \(\dot{\boldsymbol{\psi}}\) + Integrating from \({}^{\mathcal{B}}\boldsymbol{\omega}\) rather than \(\dot{\boldsymbol{\Psi}}\) makes the discrete trajectory differ slightly from integrating the Euler state directly. The difference, however, is negligible at our default frequency of 500 Hz. """ diff --git a/crazyflow/dynamics/so_rpy_rotor/__init__.py b/crazyflow/dynamics/so_rpy_rotor/__init__.py index 059b8fdc..35388268 100644 --- a/crazyflow/dynamics/so_rpy_rotor/__init__.py +++ b/crazyflow/dynamics/so_rpy_rotor/__init__.py @@ -1,27 +1,35 @@ r"""Second-order fitted RPY dynamics with first-order thrust dynamics. -Extends ``so_rpy`` by adding a scalar thrust state \(F\) that captures motor spin-up and spin-down -with a first-order lag. Rotational dynamics remain a fitted second-order linear system driven by RPY -commands. The command interface is ``[roll_rad, pitch_rad, yaw_rad, thrust_N]``. The ``rotor_vel`` -state is the current thrust in Newtons (not motor RPMs), carried as four entries of which only -the first enters the dynamics. +Extends ``so_rpy`` by adding a scalar thrust state \(f_\Sigma\) that captures motor spin-up and +spin-down with a first-order lag. Rotational dynamics remain a fitted second-order linear system +driven by RPY commands. The command interface is ``[roll_rad, pitch_rad, yaw_rad, thrust_N]``. The +``rotor_vel`` state is the current thrust in Newtons (not motor RPMs), carried as four entries of +which only the first enters the dynamics. \[ \begin{aligned} - \dot{F} &= \frac{1}{\tau}(F_{\mathrm{cmd}} - F), \\ + \dot{f}_\Sigma &= \frac{1}{c_\tau} (f_{\Sigma,\mathrm{cmd}} - f_\Sigma), \\ \dot{\mathbf{p}} &= \mathbf{v}, \\ m\dot{\mathbf{v}} &= m\mathbf{g} - + (c_{\mathrm{acc}} + c_f F)\,R\,\mathbf{e}_z, \\ - \ddot{\boldsymbol{\psi}} &= - c_{\psi}\,\boldsymbol{\psi} - + c_{\dot{\psi}}\,\dot{\boldsymbol{\psi}} - + c_u\,\mathbf{u}_{\mathrm{rpy}}, + + (c_\mathrm{acc} + c_\mathrm{f} f_\Sigma)\,\mathbf{R}\,\mathbf{e}_\mathrm{z}, \\ + \ddot{\boldsymbol{\Psi}} &= + \boldsymbol{c}_{\boldsymbol{\Psi},1}\,\boldsymbol{\Psi} + + \boldsymbol{c}_{\boldsymbol{\Psi},2}\,\dot{\boldsymbol{\Psi}} + + \boldsymbol{c}_{\boldsymbol{\Psi},3}\,\boldsymbol{\Psi}_\mathrm{cmd}, \end{aligned} \] -where \(\tau\) is the thrust time constant, \(\boldsymbol{\psi} = [\phi,\theta,\psi]^{\top}\) are -the roll/pitch/yaw angles with rates \(\dot{\boldsymbol{\psi}}\), and -\(R = {}^{\mathcal{I}}R_{\mathcal{B}}(\boldsymbol{\psi})\) is the rotation from body to world frame. +where \(f_\Sigma\) is the collective thrust, \(\mathbf{p}\) and \(\mathbf{v}\) are the position and +velocity, \(m\) is the mass, \(\mathbf{g}\) is the gravity vector, \(\mathbf{e}_\mathrm{z}\) is the +unit vector in z direction, \(\mathbf{R} = +{}^{\mathcal{I}}\mathbf{R}_{\mathcal{B}}(\boldsymbol{\Psi})\) is the rotation from body to world +frame, \(\boldsymbol{\Psi} = [\phi,\theta,\psi]^{\top}\) holds the roll, pitch, and yaw angles with +rates \(\dot{\boldsymbol{\Psi}}\), and \(f_{\Sigma,\mathrm{cmd}}\) and +\(\boldsymbol{\Psi}_\mathrm{cmd}\) are the commanded collective thrust and attitude. The thrust time +coefficient \(c_\tau\), the thrust offset \(c_\mathrm{acc}\), the thrust scaling coefficient +\(c_\mathrm{f}\), and the rotational coefficients \(\boldsymbol{c}_{\boldsymbol{\Psi},1}\), +\(\boldsymbol{c}_{\boldsymbol{\Psi},2}\), and \(\boldsymbol{c}_{\boldsymbol{\Psi},3}\) are +identified from flight data. This is the native Euler-angle form. For how the simulation integrates this state in quaternion + angular velocity coordinates, see [so_rpy][crazyflow.dynamics.so_rpy]. diff --git a/crazyflow/dynamics/so_rpy_rotor_drag/__init__.py b/crazyflow/dynamics/so_rpy_rotor_drag/__init__.py index 4113992e..0b319ae3 100644 --- a/crazyflow/dynamics/so_rpy_rotor_drag/__init__.py +++ b/crazyflow/dynamics/so_rpy_rotor_drag/__init__.py @@ -8,22 +8,29 @@ \[ \begin{aligned} - \dot{F} &= \frac{1}{\tau}(F_{\mathrm{cmd}} - F), \\ + \dot{f}_\Sigma &= \frac{1}{c_\tau} (f_{\Sigma,\mathrm{cmd}} - f_\Sigma), \\ \dot{\mathbf{p}} &= \mathbf{v}, \\ m\dot{\mathbf{v}} &= m\mathbf{g} - + (c_{\mathrm{acc}} + c_f F)\,R\,\mathbf{e}_z - + R\,D_b\,R^{\top}\mathbf{v}, \\ - \ddot{\boldsymbol{\psi}} &= - c_{\psi}\,\boldsymbol{\psi} - + c_{\dot{\psi}}\,\dot{\boldsymbol{\psi}} - + c_u\,\mathbf{u}_{\mathrm{rpy}}, + + (c_\mathrm{acc} + c_\mathrm{f} f_\Sigma)\,\mathbf{R}\,\mathbf{e}_\mathrm{z} + + \mathbf{R}\,\mathbf{C}_\mathrm{a}\,\mathbf{R}^{\top}\mathbf{v}, \\ + \ddot{\boldsymbol{\Psi}} &= + \boldsymbol{c}_{\boldsymbol{\Psi},1}\,\boldsymbol{\Psi} + + \boldsymbol{c}_{\boldsymbol{\Psi},2}\,\dot{\boldsymbol{\Psi}} + + \boldsymbol{c}_{\boldsymbol{\Psi},3}\,\boldsymbol{\Psi}_\mathrm{cmd}, \end{aligned} \] -where \(\tau\) is the thrust time constant, \(\boldsymbol{\psi} = [\phi,\theta,\psi]^{\top}\) are -the roll/pitch/yaw angles with rates \(\dot{\boldsymbol{\psi}}\), -\(R = {}^{\mathcal{I}}R_{\mathcal{B}}(\boldsymbol{\psi})\) is the rotation from body to world frame, -and \(D_b\) is the diagonal body-frame aerodynamic drag matrix. +where \(f_\Sigma\) is the collective thrust, \(\mathbf{p}\) and \(\mathbf{v}\) are the position and +velocity, \(m\) is the mass, \(\mathbf{g}\) is the gravity vector, \(\mathbf{e}_\mathrm{z}\) is the +unit vector in z direction, \(\mathbf{R} = +{}^{\mathcal{I}}\mathbf{R}_{\mathcal{B}}(\boldsymbol{\Psi})\) is the rotation from body to world +frame, \(\boldsymbol{\Psi} = [\phi,\theta,\psi]^{\top}\) holds the roll, pitch, and yaw angles with +rates \(\dot{\boldsymbol{\Psi}}\), and \(f_{\Sigma,\mathrm{cmd}}\) and +\(\boldsymbol{\Psi}_\mathrm{cmd}\) are the commanded collective thrust and attitude. The thrust time +coefficient \(c_\tau\), the diagonal body-frame drag matrix \(\mathbf{C}_\mathrm{a}\), the thrust +offset \(c_\mathrm{acc}\), the thrust scaling coefficient \(c_\mathrm{f}\), and the rotational +coefficients \(\boldsymbol{c}_{\boldsymbol{\Psi},1}\), \(\boldsymbol{c}_{\boldsymbol{\Psi},2}\), and +\(\boldsymbol{c}_{\boldsymbol{\Psi},3}\) are identified from flight data. This is the native Euler-angle form. For how the simulation integrates this state in quaternion + angular velocity coordinates, see [so_rpy][crazyflow.dynamics.so_rpy]. From 666b8fdbe55bda0465581a401f6270ddb2347b6d Mon Sep 17 00:00:00 2001 From: ratheron Date: Tue, 6 Oct 2026 20:57:57 +0200 Subject: [PATCH 2/4] Match the thrust lag convention of the paper Rename thrust_time_coef to thrust_dyn_coef, since it is a rate and no longer a time constant. --- crazyflow/dynamics/so_rpy_rotor/__init__.py | 12 ++++---- crazyflow/dynamics/so_rpy_rotor/dynamics.py | 28 +++++++++---------- crazyflow/dynamics/so_rpy_rotor/params.toml | 12 ++++---- .../dynamics/so_rpy_rotor_drag/__init__.py | 13 +++++---- .../dynamics/so_rpy_rotor_drag/dynamics.py | 28 +++++++++---------- .../dynamics/so_rpy_rotor_drag/params.toml | 12 ++++---- crazyflow/dynamics/utils/identification.py | 10 +++---- .../user-guide/dynamics/dynamics-functions.md | 4 +-- .../dynamics/system-identification.md | 6 ++-- examples/control/sampling.py | 4 +-- 10 files changed, 65 insertions(+), 64 deletions(-) diff --git a/crazyflow/dynamics/so_rpy_rotor/__init__.py b/crazyflow/dynamics/so_rpy_rotor/__init__.py index 35388268..301abfe8 100644 --- a/crazyflow/dynamics/so_rpy_rotor/__init__.py +++ b/crazyflow/dynamics/so_rpy_rotor/__init__.py @@ -8,7 +8,7 @@ \[ \begin{aligned} - \dot{f}_\Sigma &= \frac{1}{c_\tau} (f_{\Sigma,\mathrm{cmd}} - f_\Sigma), \\ + \dot{f}_\Sigma &= c_\tau (f_{\Sigma,\mathrm{cmd}} - f_\Sigma), \\ \dot{\mathbf{p}} &= \mathbf{v}, \\ m\dot{\mathbf{v}} &= m\mathbf{g} + (c_\mathrm{acc} + c_\mathrm{f} f_\Sigma)\,\mathbf{R}\,\mathbf{e}_\mathrm{z}, \\ @@ -25,11 +25,11 @@ {}^{\mathcal{I}}\mathbf{R}_{\mathcal{B}}(\boldsymbol{\Psi})\) is the rotation from body to world frame, \(\boldsymbol{\Psi} = [\phi,\theta,\psi]^{\top}\) holds the roll, pitch, and yaw angles with rates \(\dot{\boldsymbol{\Psi}}\), and \(f_{\Sigma,\mathrm{cmd}}\) and -\(\boldsymbol{\Psi}_\mathrm{cmd}\) are the commanded collective thrust and attitude. The thrust time -coefficient \(c_\tau\), the thrust offset \(c_\mathrm{acc}\), the thrust scaling coefficient -\(c_\mathrm{f}\), and the rotational coefficients \(\boldsymbol{c}_{\boldsymbol{\Psi},1}\), -\(\boldsymbol{c}_{\boldsymbol{\Psi},2}\), and \(\boldsymbol{c}_{\boldsymbol{\Psi},3}\) are -identified from flight data. +\(\boldsymbol{\Psi}_\mathrm{cmd}\) are the commanded collective thrust and attitude. The thrust +dynamics coefficient \(c_\tau\), the thrust offset \(c_\mathrm{acc}\), the thrust scaling +coefficient \(c_\mathrm{f}\), and the rotational coefficients +\(\boldsymbol{c}_{\boldsymbol{\Psi},1}\), \(\boldsymbol{c}_{\boldsymbol{\Psi},2}\), and +\(\boldsymbol{c}_{\boldsymbol{\Psi},3}\) are identified from flight data. This is the native Euler-angle form. For how the simulation integrates this state in quaternion + angular velocity coordinates, see [so_rpy][crazyflow.dynamics.so_rpy]. diff --git a/crazyflow/dynamics/so_rpy_rotor/dynamics.py b/crazyflow/dynamics/so_rpy_rotor/dynamics.py index 4bb3ad68..4c1c95c9 100644 --- a/crazyflow/dynamics/so_rpy_rotor/dynamics.py +++ b/crazyflow/dynamics/so_rpy_rotor/dynamics.py @@ -52,7 +52,7 @@ def dynamics( gravity_vec: Array, J: Array, J_inv: Array, - thrust_time_coef: Array, + thrust_dyn_coef: Array, acc_coef: Array, cmd_f_coef: Array, rpy_coef: Array, @@ -81,7 +81,7 @@ def dynamics( [0, 0, -9.81]. J: Inertia matrix (kg m^2). J_inv: Inverse inertia matrix (1/kg m^2). - thrust_time_coef: Coefficient for the rotor dynamics (1/s). + thrust_dyn_coef: Coefficient for the rotor dynamics (1/s). acc_coef: Coefficient for the acceleration (1/s^2). cmd_f_coef: Coefficient for the collective thrust (N/rad^2). rpy_coef: Coefficient for the roll pitch yaw dynamics (1/s). @@ -109,7 +109,7 @@ def dynamics( rotor_vel, mass=mass, gravity_vec=gravity_vec, - thrust_time_coef=thrust_time_coef, + thrust_dyn_coef=thrust_dyn_coef, acc_coef=acc_coef, cmd_f_coef=cmd_f_coef, rpy_coef=rpy_coef, @@ -146,7 +146,7 @@ def dynamics_euler( *, mass: float, gravity_vec: Array, - thrust_time_coef: Array, + thrust_dyn_coef: Array, acc_coef: Array, cmd_f_coef: Array, rpy_coef: Array, @@ -157,7 +157,7 @@ def dynamics_euler( xp = array_namespace(pos) device = xp_device(pos) mass, gravity_vec = to_xp(mass, gravity_vec, xp=xp, device=device) - thrust_time_coef, acc_coef = to_xp(thrust_time_coef, acc_coef, xp=xp, device=device) + thrust_dyn_coef, acc_coef = to_xp(thrust_dyn_coef, acc_coef, xp=xp, device=device) cmd_f_coef, rpy_coef = to_xp(cmd_f_coef, rpy_coef, xp=xp, device=device) rpy_rates_coef, cmd_rpy_coef = to_xp(rpy_rates_coef, cmd_rpy_coef, xp=xp, device=device) cmd_f = cmd[..., -1] @@ -167,7 +167,7 @@ def dynamics_euler( warnings.warn("Rotor velocity not provided, using commanded rotor velocity.") rotor_vel, rotor_vel_dot = cmd_f[..., None], None else: - rotor_vel_dot = (cmd_f[..., None] - rotor_vel) / thrust_time_coef + rotor_vel_dot = thrust_dyn_coef * (cmd_f[..., None] - rotor_vel) forces_motor = rotor_vel[..., 0:1] # (..., 1) thrust = acc_coef + cmd_f_coef * forces_motor drone_z_axis = R.from_euler("xyz", rpy).as_matrix()[..., -1] @@ -186,7 +186,7 @@ def symbolic_dynamics( gravity_vec: Array, J: Array, J_inv: Array, - thrust_time_coef: Array, + thrust_dyn_coef: Array, acc_coef: Array, cmd_f_coef: Array, rpy_coef: Array, @@ -209,7 +209,7 @@ def symbolic_dynamics( gravity_vec: Gravity vector, shape ``(3,)``. J: Inertia matrix, shape ``(3, 3)``. J_inv: Inverse inertia matrix, shape ``(3, 3)``. - thrust_time_coef: First-order thrust lag time constant coefficient (1/s). + thrust_dyn_coef: First-order thrust lag coefficient (1/s). acc_coef: Scalar acceleration offset coefficient. cmd_f_coef: Collective-thrust-to-acceleration coefficient. rpy_coef: RPY state feedback coefficient, shape ``(3,)``. @@ -240,7 +240,7 @@ def symbolic_dynamics( gravity_vec=gravity_vec, J=J, J_inv=J_inv, - thrust_time_coef=thrust_time_coef, + thrust_dyn_coef=thrust_dyn_coef, acc_coef=acc_coef, cmd_f_coef=cmd_f_coef, rpy_coef=rpy_coef, @@ -300,7 +300,7 @@ def symbolic_dynamics_euler( gravity_vec: Array, J: Array, J_inv: Array, - thrust_time_coef: Array, + thrust_dyn_coef: Array, acc_coef: Array, cmd_f_coef: Array, rpy_coef: Array, @@ -320,7 +320,7 @@ def symbolic_dynamics_euler( gravity_vec: Gravity vector, shape ``(3,)``. J: Inertia matrix, shape ``(3, 3)``. J_inv: Inverse inertia matrix, shape ``(3, 3)``. - thrust_time_coef: First-order thrust lag time constant coefficient (1/s). + thrust_dyn_coef: First-order thrust lag coefficient (1/s). acc_coef: Scalar acceleration offset coefficient. cmd_f_coef: Collective-thrust-to-acceleration coefficient. rpy_coef: RPY state feedback coefficient, shape ``(3,)``. @@ -349,7 +349,7 @@ def symbolic_dynamics_euler( # Defining the dynamics function # Note that we are abusing the rotor_vel state as the thrust if model_rotor_vel: - rotor_vel_dot = 1 / thrust_time_coef * (cmd_thrust - symbols.rotor_vel) + rotor_vel_dot = thrust_dyn_coef * (cmd_thrust - symbols.rotor_vel) forces_motor = symbols.rotor_vel[0] # We are only using the first element else: forces_motor = cmd_thrust @@ -382,7 +382,7 @@ class Params: """Inertia matrix of the drone.""" J_inv: Array = field(metadata={CORE_NDIM_KEY: 2}) # (3, 3) """Inverse of the inertia matrix of the drone.""" - thrust_time_coef: Array = field(metadata={CORE_NDIM_KEY: 1}) # (1,) + thrust_dyn_coef: Array = field(metadata={CORE_NDIM_KEY: 1}) # (1,) """Rotor coefficient of the drone.""" acc_coef: Array = field(metadata={CORE_NDIM_KEY: 1}) # (1,) """Acceleration coefficient of the drone.""" @@ -405,7 +405,7 @@ def create(drone: str, device: Device) -> Params: gravity_vec=jnp.asarray(p["gravity_vec"], device=device), J=J, J_inv=jnp.linalg.inv(J), - thrust_time_coef=jnp.asarray([p["thrust_time_coef"]], device=device), + thrust_dyn_coef=jnp.asarray([p["thrust_dyn_coef"]], device=device), acc_coef=jnp.asarray([p["acc_coef"]], device=device), cmd_f_coef=jnp.asarray([p["cmd_f_coef"]], device=device), rpy_coef=jnp.asarray(p["rpy_coef"], device=device), diff --git a/crazyflow/dynamics/so_rpy_rotor/params.toml b/crazyflow/dynamics/so_rpy_rotor/params.toml index ae42deea..07da8c90 100644 --- a/crazyflow/dynamics/so_rpy_rotor/params.toml +++ b/crazyflow/dynamics/so_rpy_rotor/params.toml @@ -14,7 +14,7 @@ # rpy_coef = [0.0, 0.0, 0.0] # rpy_rates_coef = [0.0, 0.0, 0.0] # cmd_rpy_coef = [0.0, 0.0, 0.0] -# thrust_time_coef = 0.0 # s +# thrust_dyn_coef = 0.0 # 1/s # # Identify the coefficients from flight data with the system identification pipeline (see # docs/user-guide/dynamics/system-identification.md). @@ -30,7 +30,7 @@ thrust_min = 0.012817578393224994 # in N per motor thrust_max = 0.12 # in N per motor acc_coef = 0.0 cmd_f_coef = 1.0242686698819605 -thrust_time_coef = 0.08671854102279604 +thrust_dyn_coef = 11.531559320597035 rpy_coef = [-485.8620950386863, -485.8620950386863, -333.2438787006866] rpy_rates_coef = [-33.17729279340664, -33.17729279340664, -39.535506757052424] cmd_rpy_coef = [448.0183990789311, 448.0183990789311, 306.31685518928396] @@ -47,7 +47,7 @@ thrust_min = 0.012817578393224994 # in N per motor thrust_max = 0.12 # in N per motor acc_coef = 0.0 cmd_f_coef = 0.98323006 -thrust_time_coef = 0.0578952 +thrust_dyn_coef = 17.272589092014535 rpy_coef = [-319.14, -319.14, -284.28] rpy_rates_coef = [-20.85, -20.85, -38.43] cmd_rpy_coef = [263.30, 263.30, 502.58] @@ -64,7 +64,7 @@ thrust_min = 0.01922636758983749 # in N per motor thrust_max = 0.18 # in N per motor acc_coef = 0.0 cmd_f_coef = 1.0145454356801935 -thrust_time_coef = 0.05941543454583637 +thrust_dyn_coef = 16.830643546476875 rpy_coef = [-373.81009672301474, -373.81009672301474, -262.01237938054936] rpy_rates_coef = [-29.447753470026274, -29.447753470026274, -29.745699818936384] cmd_rpy_coef = [350.209624193645, 350.209624193645, 241.085024111866] @@ -81,7 +81,7 @@ thrust_min = 0.02136263065537499 # in N per motor thrust_max = 0.2 # in N per motor acc_coef = 0.0 cmd_f_coef = 0.9472350463278153 -thrust_time_coef = 0.05180073413161973 +thrust_dyn_coef = 19.304745709956823 rpy_coef = [-156.34812197843243, -156.34812197843243, -144.7222372741053] rpy_rates_coef = [-16.300330418164553, -16.300330418164553, -17.368699336318954] cmd_rpy_coef = [139.7494294272397, 139.7494294272397, 127.01037564242333] @@ -97,7 +97,7 @@ thrust_min = 1.0 # in N per motor thrust_max = 12.13 # in N per motor acc_coef = 0.0 cmd_f_coef = 0.9122876298664497 -thrust_time_coef = 0.04493428195421334 +thrust_dyn_coef = 22.254723042397103 rpy_coef = [-51.42302192586755, -51.42302192586755, -26.929070173696157] rpy_rates_coef = [-8.93635377240008, -8.93635377240008, -8.646484332625702] cmd_rpy_coef = [47.3447720777806, 47.3447720777806, 25.310886504404653] diff --git a/crazyflow/dynamics/so_rpy_rotor_drag/__init__.py b/crazyflow/dynamics/so_rpy_rotor_drag/__init__.py index 0b319ae3..8545571e 100644 --- a/crazyflow/dynamics/so_rpy_rotor_drag/__init__.py +++ b/crazyflow/dynamics/so_rpy_rotor_drag/__init__.py @@ -8,7 +8,7 @@ \[ \begin{aligned} - \dot{f}_\Sigma &= \frac{1}{c_\tau} (f_{\Sigma,\mathrm{cmd}} - f_\Sigma), \\ + \dot{f}_\Sigma &= c_\tau (f_{\Sigma,\mathrm{cmd}} - f_\Sigma), \\ \dot{\mathbf{p}} &= \mathbf{v}, \\ m\dot{\mathbf{v}} &= m\mathbf{g} + (c_\mathrm{acc} + c_\mathrm{f} f_\Sigma)\,\mathbf{R}\,\mathbf{e}_\mathrm{z} @@ -26,11 +26,12 @@ {}^{\mathcal{I}}\mathbf{R}_{\mathcal{B}}(\boldsymbol{\Psi})\) is the rotation from body to world frame, \(\boldsymbol{\Psi} = [\phi,\theta,\psi]^{\top}\) holds the roll, pitch, and yaw angles with rates \(\dot{\boldsymbol{\Psi}}\), and \(f_{\Sigma,\mathrm{cmd}}\) and -\(\boldsymbol{\Psi}_\mathrm{cmd}\) are the commanded collective thrust and attitude. The thrust time -coefficient \(c_\tau\), the diagonal body-frame drag matrix \(\mathbf{C}_\mathrm{a}\), the thrust -offset \(c_\mathrm{acc}\), the thrust scaling coefficient \(c_\mathrm{f}\), and the rotational -coefficients \(\boldsymbol{c}_{\boldsymbol{\Psi},1}\), \(\boldsymbol{c}_{\boldsymbol{\Psi},2}\), and -\(\boldsymbol{c}_{\boldsymbol{\Psi},3}\) are identified from flight data. +\(\boldsymbol{\Psi}_\mathrm{cmd}\) are the commanded collective thrust and attitude. The thrust +dynamics coefficient \(c_\tau\), the diagonal body-frame drag matrix \(\mathbf{C}_\mathrm{a}\), the +thrust offset \(c_\mathrm{acc}\), the thrust scaling coefficient \(c_\mathrm{f}\), and the +rotational coefficients \(\boldsymbol{c}_{\boldsymbol{\Psi},1}\), +\(\boldsymbol{c}_{\boldsymbol{\Psi},2}\), and \(\boldsymbol{c}_{\boldsymbol{\Psi},3}\) are +identified from flight data. This is the native Euler-angle form. For how the simulation integrates this state in quaternion + angular velocity coordinates, see [so_rpy][crazyflow.dynamics.so_rpy]. diff --git a/crazyflow/dynamics/so_rpy_rotor_drag/dynamics.py b/crazyflow/dynamics/so_rpy_rotor_drag/dynamics.py index 103ba270..206d0afb 100644 --- a/crazyflow/dynamics/so_rpy_rotor_drag/dynamics.py +++ b/crazyflow/dynamics/so_rpy_rotor_drag/dynamics.py @@ -63,7 +63,7 @@ def dynamics( gravity_vec: Array, J: Array, J_inv: Array, - thrust_time_coef: Array, + thrust_dyn_coef: Array, acc_coef: Array, cmd_f_coef: Array, rpy_coef: Array, @@ -93,7 +93,7 @@ def dynamics( [0, 0, -9.81]. J: Inertia matrix (kg m^2). J_inv: Inverse inertia matrix (1/kg m^2). - thrust_time_coef: Coefficient for the rotor dynamics (1/s). + thrust_dyn_coef: Coefficient for the rotor dynamics (1/s). acc_coef: Coefficient for the acceleration (1/s^2). cmd_f_coef: Coefficient for the collective thrust (N/rad^2). rpy_coef: Coefficient for the roll pitch yaw dynamics (1/s). @@ -122,7 +122,7 @@ def dynamics( rotor_vel, mass=mass, gravity_vec=gravity_vec, - thrust_time_coef=thrust_time_coef, + thrust_dyn_coef=thrust_dyn_coef, acc_coef=acc_coef, cmd_f_coef=cmd_f_coef, rpy_coef=rpy_coef, @@ -160,7 +160,7 @@ def dynamics_euler( *, mass: float, gravity_vec: Array, - thrust_time_coef: Array, + thrust_dyn_coef: Array, acc_coef: Array, cmd_f_coef: Array, rpy_coef: Array, @@ -172,7 +172,7 @@ def dynamics_euler( xp = array_namespace(pos) device = xp_device(pos) mass, gravity_vec = to_xp(mass, gravity_vec, xp=xp, device=device) - thrust_time_coef, acc_coef = to_xp(thrust_time_coef, acc_coef, xp=xp, device=device) + thrust_dyn_coef, acc_coef = to_xp(thrust_dyn_coef, acc_coef, xp=xp, device=device) cmd_f_coef, rpy_coef = to_xp(cmd_f_coef, rpy_coef, xp=xp, device=device) rpy_rates_coef, cmd_rpy_coef = to_xp(rpy_rates_coef, cmd_rpy_coef, xp=xp, device=device) drag_matrix = to_xp(drag_matrix, xp=xp, device=device) @@ -183,7 +183,7 @@ def dynamics_euler( warnings.warn("Rotor velocity not provided, using commanded rotor velocity.") rotor_vel, rotor_vel_dot = cmd_f[..., None], None else: - rotor_vel_dot = (cmd_f[..., None] - rotor_vel) / thrust_time_coef + rotor_vel_dot = thrust_dyn_coef * (cmd_f[..., None] - rotor_vel) forces_motor = rotor_vel[..., 0:1] # (..., 1) thrust = acc_coef + cmd_f_coef * forces_motor rot_mat = R.from_euler("xyz", rpy).inv().as_matrix() # rotation from world to body @@ -207,7 +207,7 @@ def symbolic_dynamics( gravity_vec: Array, J: Array, J_inv: Array, - thrust_time_coef: Array, + thrust_dyn_coef: Array, acc_coef: Array, cmd_f_coef: Array, rpy_coef: Array, @@ -231,7 +231,7 @@ def symbolic_dynamics( gravity_vec: Gravity vector, shape ``(3,)``. J: Inertia matrix, shape ``(3, 3)``. J_inv: Inverse inertia matrix, shape ``(3, 3)``. - thrust_time_coef: First-order thrust lag time constant coefficient (1/s). + thrust_dyn_coef: First-order thrust lag coefficient (1/s). acc_coef: Scalar acceleration offset coefficient. cmd_f_coef: Collective-thrust-to-acceleration coefficient. rpy_coef: RPY state feedback coefficient, shape ``(3,)``. @@ -266,7 +266,7 @@ def symbolic_dynamics( gravity_vec=gravity_vec, J=J, J_inv=J_inv, - thrust_time_coef=thrust_time_coef, + thrust_dyn_coef=thrust_dyn_coef, acc_coef=acc_coef, cmd_f_coef=cmd_f_coef, rpy_coef=rpy_coef, @@ -327,7 +327,7 @@ def symbolic_dynamics_euler( gravity_vec: Array, J: Array, J_inv: Array, - thrust_time_coef: Array, + thrust_dyn_coef: Array, acc_coef: Array, cmd_f_coef: Array, rpy_coef: Array, @@ -348,7 +348,7 @@ def symbolic_dynamics_euler( gravity_vec: Gravity vector, shape ``(3,)``. J: Inertia matrix, shape ``(3, 3)``. J_inv: Inverse inertia matrix, shape ``(3, 3)``. - thrust_time_coef: First-order thrust lag time constant coefficient (1/s). + thrust_dyn_coef: First-order thrust lag coefficient (1/s). acc_coef: Scalar acceleration offset coefficient. cmd_f_coef: Collective-thrust-to-acceleration coefficient. rpy_coef: RPY state feedback coefficient, shape ``(3,)``. @@ -379,7 +379,7 @@ def symbolic_dynamics_euler( # Defining the dynamics function # Note that we are abusing the rotor_vel state as the thrust if model_rotor_vel: - rotor_vel_dot = 1 / thrust_time_coef * (cmd_thrust - symbols.rotor_vel) + rotor_vel_dot = thrust_dyn_coef * (cmd_thrust - symbols.rotor_vel) forces_motor = symbols.rotor_vel[0] # We are only using the first element else: forces_motor = cmd_thrust @@ -416,7 +416,7 @@ class Params: """Inertia matrix of the drone.""" J_inv: Array = field(metadata={CORE_NDIM_KEY: 2}) # (3, 3) """Inverse of the inertia matrix of the drone.""" - thrust_time_coef: Array = field(metadata={CORE_NDIM_KEY: 1}) # (1,) + thrust_dyn_coef: Array = field(metadata={CORE_NDIM_KEY: 1}) # (1,) """Rotor coefficient of the drone.""" acc_coef: Array = field(metadata={CORE_NDIM_KEY: 1}) # (1,) """Acceleration coefficient of the drone.""" @@ -441,7 +441,7 @@ def create(drone: str, device: Device) -> Params: gravity_vec=jnp.asarray(p["gravity_vec"], device=device), J=J, J_inv=jnp.linalg.inv(J), - thrust_time_coef=jnp.asarray([p["thrust_time_coef"]], device=device), + thrust_dyn_coef=jnp.asarray([p["thrust_dyn_coef"]], device=device), acc_coef=jnp.asarray([p["acc_coef"]], device=device), cmd_f_coef=jnp.asarray([p["cmd_f_coef"]], device=device), rpy_coef=jnp.asarray(p["rpy_coef"], device=device), diff --git a/crazyflow/dynamics/so_rpy_rotor_drag/params.toml b/crazyflow/dynamics/so_rpy_rotor_drag/params.toml index 749b1ac5..f4097b4d 100644 --- a/crazyflow/dynamics/so_rpy_rotor_drag/params.toml +++ b/crazyflow/dynamics/so_rpy_rotor_drag/params.toml @@ -14,7 +14,7 @@ # rpy_coef = [0.0, 0.0, 0.0] # rpy_rates_coef = [0.0, 0.0, 0.0] # cmd_rpy_coef = [0.0, 0.0, 0.0] -# thrust_time_coef = 0.0 # s +# thrust_dyn_coef = 0.0 # 1/s # drag_matrix = [ # 1/s # [0.0, 0.0, 0.0], # [0.0, 0.0, 0.0], @@ -35,7 +35,7 @@ thrust_min = 0.012817578393224994 # in N per motor thrust_max = 0.12 # in N per motor acc_coef = 0.0 cmd_f_coef = 1.0322435843278281 -thrust_time_coef = 0.14876008226610188 +thrust_dyn_coef = 6.722233443049602 drag_matrix = [ [-0.014953790232821078, 0.0, 0.0], [0.0, -0.014953790232821078, 0.0], @@ -57,7 +57,7 @@ thrust_min = 0.012817578393224994 # in N per motor thrust_max = 0.12 # in N per motor acc_coef = 0.0 cmd_f_coef = 0.99085062 -thrust_time_coef = 0.11554009 +thrust_dyn_coef = 8.6550045096901 drag_matrix = [ [-0.01351483, 0.0, 0.0 ], [0.0, -0.01351483, 0.0 ], @@ -79,7 +79,7 @@ thrust_min = 0.01922636758983749 # in N per motor thrust_max = 0.18 # in N per motor acc_coef = 0.0 cmd_f_coef = 1.0229179982077607 -thrust_time_coef = 0.17585713583659168 +thrust_dyn_coef = 5.686434020676935 drag_matrix = [ [-0.015203241038199845, 0.0, 0.0], [0.0, -0.015203241038199845, 0.0], @@ -101,7 +101,7 @@ thrust_min = 0.02136263065537499 # in N per motor thrust_max = 0.2 # in N per motor acc_coef = 0.0 cmd_f_coef = 0.959471532998666 -thrust_time_coef = 0.08824147411162254 +thrust_dyn_coef = 11.332539603033299 drag_matrix = [ [-0.021643637770852733, 0.0, 0.0], [0.0, -0.021643637770852733, 0.0], @@ -122,7 +122,7 @@ thrust_min = 1.0 # in N per motor thrust_max = 12.13 # in N per motor acc_coef = 0.0 cmd_f_coef = 0.9234799918739313 -thrust_time_coef = 0.07111262651857447 +thrust_dyn_coef = 14.062200328640682 drag_matrix = [ [-0.8031707417931839, 0.0, 0.0 ], [0.0, -0.8031707417931839, 0.0 ], diff --git a/crazyflow/dynamics/utils/identification.py b/crazyflow/dynamics/utils/identification.py index 664d1309..033079ce 100644 --- a/crazyflow/dynamics/utils/identification.py +++ b/crazyflow/dynamics/utils/identification.py @@ -36,7 +36,7 @@ dynamics_euler, mass=0.1, gravity_vec=jnp.array([0, 0, -9.81]), - thrust_time_coef=0.1, + thrust_dyn_coef=10.0, acc_coef=0.0, drag_matrix=jnp.zeros((3, 3)), cmd_f_coef=1.0, @@ -62,7 +62,7 @@ def _simulate_system_translation( vel: Velocity of the drone (N, 3) cmd_f: Commanded thrust (N,) t: Time samples (N,) - params: Dynamics parameters [cmd_f_coef, thrust_time_coef, drag_xy_coef, drag_z_coef] + params: Dynamics parameters [cmd_f_coef, thrust_dyn_coef, drag_xy_coef, drag_z_coef] constants: Additional constants (mass, gravity_vec, etc.) returns: predicted acceleration (N, 3) @@ -87,7 +87,7 @@ def _step_thrust(carry: Array, inputs: tuple) -> tuple: rotor_vel=carry, mass=constants["mass"], gravity_vec=constants["gravity_vec"], - thrust_time_coef=params[1], + thrust_dyn_coef=params[1], acc_coef=0.0, drag_matrix=jnp.diag(jnp.array([params[2], params[2], params[3]])), cmd_f_coef=params[0], @@ -110,7 +110,7 @@ def _step_thrust(carry: Array, inputs: tuple) -> tuple: rotor_vel=thrusts[..., None], mass=constants["mass"], gravity_vec=constants["gravity_vec"], - thrust_time_coef=params[1], + thrust_dyn_coef=params[1], acc_coef=0.0, drag_matrix=jnp.diag(jnp.array([params[2], params[2], params[3]])), cmd_f_coef=params[0], @@ -241,7 +241,7 @@ def sys_id_translation( theta = res.x params = {"cmd_f_coef": theta[0]} if "rotor" in dynamics: - params["thrust_time_coef"] = theta[1] + params["thrust_dyn_coef"] = theta[1] else: theta[1] = 0.0 if "drag" in dynamics: diff --git a/docs/user-guide/dynamics/dynamics-functions.md b/docs/user-guide/dynamics/dynamics-functions.md index 5d49a1bf..a1f6261a 100644 --- a/docs/user-guide/dynamics/dynamics-functions.md +++ b/docs/user-guide/dynamics/dynamics-functions.md @@ -16,8 +16,8 @@ What differs between dynamics is the command interface, which parameters are nee | Module | `cmd` input | Rotor dynamics | Key added params | |---|---|---|---| | `first_principles` | Motor RPMs `(4,)` | Yes | `rpm2thrust`, `rpm2torque`, `mixing_matrix`, `L`, `prop_inertia` | -| `so_rpy_rotor_drag` | rpyt `(4,)` | Yes | `thrust_time_coef`, `drag_matrix` | -| `so_rpy_rotor` | rpyt `(4,)` | Yes | `thrust_time_coef` | +| `so_rpy_rotor_drag` | rpyt `(4,)` | Yes | `thrust_dyn_coef`, `drag_matrix` | +| `so_rpy_rotor` | rpyt `(4,)` | Yes | `thrust_dyn_coef` | | `so_rpy` | rpyt `(4,)` | No | — | ## first_principles diff --git a/docs/user-guide/dynamics/system-identification.md b/docs/user-guide/dynamics/system-identification.md index 72aeefef..f0d6542f 100644 --- a/docs/user-guide/dynamics/system-identification.md +++ b/docs/user-guide/dynamics/system-identification.md @@ -48,7 +48,7 @@ trans_params = sys_id_translation( verbose=0, # 0 = silent, 1 = progress, 2 = full optimizer output plot=True, # show fit vs. measured plots ) -# Returns: {'cmd_f_coef': ..., 'thrust_time_coef': ..., +# Returns: {'cmd_f_coef': ..., 'thrust_dyn_coef': ..., # 'drag_xy_coef': ..., 'drag_z_coef': ...} # Step 5 — fit rotational parameters @@ -85,7 +85,7 @@ Once you have the identified coefficients, add them to the relevant `params.toml ```toml [my_drone] cmd_f_coef = 1.032 # from trans_params["cmd_f_coef"] -thrust_time_coef = 0.149 # from trans_params["thrust_time_coef"] +thrust_dyn_coef = 6.72 # from trans_params["thrust_dyn_coef"] drag_matrix = [[-0.0150, 0.0, 0.0], [0.0, -0.0150, 0.0], [0.0, 0.0, -0.0139]] # diag([drag_xy, drag_xy, drag_z]) @@ -114,7 +114,7 @@ Support for new drones can be added to the shared parameter files via a pull req Choose based on which physical effects you need to capture: - **`so_rpy`** — identifies only `cmd_f_coef`; no motor dynamics, no drag. Fastest to calibrate, good for slow flight. -- **`so_rpy_rotor`** — adds `thrust_time_coef` to model motor spin-up delay. Better for agile maneuvers. +- **`so_rpy_rotor`** — adds `thrust_dyn_coef` to model motor spin-up delay. Better for agile maneuvers. - **`so_rpy_rotor_drag`** — adds `drag_xy_coef` and `drag_z_coef`. Best accuracy at higher speeds where aerodynamic drag is significant. --- diff --git a/examples/control/sampling.py b/examples/control/sampling.py index ec06f066..ee870197 100644 --- a/examples/control/sampling.py +++ b/examples/control/sampling.py @@ -253,7 +253,7 @@ def main() -> None: rollout_simulator.reset() thrust_estimate = hover_thrust_value # Initial thrust estimate - thrust_time_coef = float(rollout_simulator.data.params.thrust_time_coef[0]) + thrust_dyn_coef = float(rollout_simulator.data.params.thrust_dyn_coef[0]) hover_cmd = jax.device_put( jnp.array([0.0, 0.0, 0.0, hover_thrust_value], dtype=jnp.float32), controller_device ) @@ -305,7 +305,7 @@ def main() -> None: action, key, mean_controls, best_positions, sampled_positions = control( t, obs, key, mean_controls, controller_fn, controller_device ) - thrust_estimate += (action[3] - thrust_estimate) / thrust_time_coef / CTRL_FREQ + thrust_estimate += thrust_dyn_coef * (action[3] - thrust_estimate) / CTRL_FREQ sim.attitude_control(action[None, None]) sim.step(sim.freq // CTRL_FREQ) position_history.append(np.asarray(sim.data.states.pos[0, 0])) From 3c5559a15f66444d0643c25655e138f12a103fac Mon Sep 17 00:00:00 2001 From: ratheron Date: Tue, 6 Oct 2026 20:52:54 +0200 Subject: [PATCH 3/4] Add dynamics equations page linking the API reference --- docs/user-guide/dynamics/equations.md | 8 ++++++++ properdocs.yml | 1 + 2 files changed, 9 insertions(+) create mode 100644 docs/user-guide/dynamics/equations.md diff --git a/docs/user-guide/dynamics/equations.md b/docs/user-guide/dynamics/equations.md new file mode 100644 index 00000000..be675b18 --- /dev/null +++ b/docs/user-guide/dynamics/equations.md @@ -0,0 +1,8 @@ +# Equations + +The equations of motion of each dynamics model, including all variables and parameters, are documented in the API reference: + +- [First principles][crazyflow.dynamics.first_principles] (`Dynamics.first_principles`) +- [SO(3) + RPY][crazyflow.dynamics.so_rpy] (`Dynamics.so_rpy`) +- [SO(3) + RPY + rotor][crazyflow.dynamics.so_rpy_rotor] (`Dynamics.so_rpy_rotor`) +- [SO(3) + RPY + rotor + drag][crazyflow.dynamics.so_rpy_rotor_drag] (`Dynamics.so_rpy_rotor_drag`) diff --git a/properdocs.yml b/properdocs.yml index d09b19bd..cb9a790e 100644 --- a/properdocs.yml +++ b/properdocs.yml @@ -56,6 +56,7 @@ nav: - Functional API: user-guide/functional-api.md - Dynamics: - user-guide/dynamics/index.md + - Equations: user-guide/dynamics/equations.md - Dynamics functions: user-guide/dynamics/dynamics-functions.md - Parametrization: user-guide/dynamics/parametrize.md - Batching & domain randomization: user-guide/dynamics/batching.md From fb137f15bde9922b38879875df32529994b27d75 Mon Sep 17 00:00:00 2001 From: ratheron Date: Wed, 7 Oct 2026 14:10:56 +0200 Subject: [PATCH 4/4] Simplify docs --- .../dynamics/first_principles/__init__.py | 125 ++++++++++-------- .../dynamics/first_principles/dynamics.py | 55 ++++---- .../dynamics/first_principles/params.toml | 2 +- crazyflow/dynamics/so_rpy/__init__.py | 39 ++++-- crazyflow/dynamics/so_rpy/dynamics.py | 47 ++++--- crazyflow/dynamics/so_rpy_rotor/__init__.py | 42 ++++-- crazyflow/dynamics/so_rpy_rotor/dynamics.py | 55 ++++---- .../dynamics/so_rpy_rotor_drag/__init__.py | 49 ++++--- .../dynamics/so_rpy_rotor_drag/dynamics.py | 65 ++++----- .../dynamics/so_rpy_rotor_drag/params.toml | 2 +- 10 files changed, 284 insertions(+), 197 deletions(-) diff --git a/crazyflow/dynamics/first_principles/__init__.py b/crazyflow/dynamics/first_principles/__init__.py index 57072dbe..1224ba4f 100644 --- a/crazyflow/dynamics/first_principles/__init__.py +++ b/crazyflow/dynamics/first_principles/__init__.py @@ -6,34 +6,28 @@ taken from CAD data. The thrust and torque curves are fitted to load cell data, and the inertia and drag coefficients are identified from flight data. -Motor forces and torques are quadratic polynomials in RPM: +When rotor dynamics are modelled, the rotor speeds evolve as: \[ - f_{\mathrm{m},i} = k_0 + k_1 \Omega_i + k_2 \Omega_i^2, \qquad - t_{\mathrm{m},i} = m_0 + m_1 \Omega_i + m_2 \Omega_i^2, + \dot{\boldsymbol{\Omega}} = \begin{cases} + \hat{c}_\mathrm{v} (\boldsymbol{\Omega}_\mathrm{cmd} - \boldsymbol{\Omega}) + + \hat{c}_\mathrm{d} (\boldsymbol{\Omega}_\mathrm{cmd}^2 - \boldsymbol{\Omega}^2) + & \forall\, \boldsymbol{\Omega}_\mathrm{cmd} \geq \boldsymbol{\Omega}, \\ + \check{c}_\mathrm{v} (\boldsymbol{\Omega}_\mathrm{cmd} - \boldsymbol{\Omega}) + + \check{c}_\mathrm{d} (\boldsymbol{\Omega}_\mathrm{cmd}^2 - \boldsymbol{\Omega}^2) + & \forall\, \boldsymbol{\Omega}_\mathrm{cmd} < \boldsymbol{\Omega}. + \end{cases} \] -where \(\Omega_i\) is the rotor speed of motor \(i\) in RPM, \(f_{\mathrm{m},i}\) and -\(t_{\mathrm{m},i}\) are its thrust and drag torque, and \(k_0, k_1, k_2\) and \(m_0, m_1, m_2\) are -the thrust and torque curve coefficients. - -When rotor dynamics are modelled, each motor RPM evolves as: +Each motor produces a thrust and a drag torque, both quadratic polynomials of its rotor speed in +RPM: \[ - \dot{\Omega}_i = \begin{cases} - \hat{c}_\mathrm{v} (\Omega_{\mathrm{cmd},i} - \Omega_i) - + \hat{c}_\mathrm{d} (\Omega_{\mathrm{cmd},i}^2 - \Omega_i^2) - & \Omega_{\mathrm{cmd},i} \geq \Omega_i \\[4pt] - \check{c}_\mathrm{v} (\Omega_{\mathrm{cmd},i} - \Omega_i) - + \check{c}_\mathrm{d} (\Omega_{\mathrm{cmd},i}^2 - \Omega_i^2) - & \Omega_{\mathrm{cmd},i} < \Omega_i - \end{cases} + f_{\mathrm{m},i} = k_{\mathrm{f},0} + k_{\mathrm{f},1} \Omega_i + k_{\mathrm{f},2} \Omega_i^2, + \qquad + t_{\mathrm{m},i} = k_{\mathrm{t},0} + k_{\mathrm{t},1} \Omega_i + k_{\mathrm{t},2} \Omega_i^2. \] -where \(\Omega_{\mathrm{cmd},i}\) is the commanded rotor speed, \(\hat{c}_\mathrm{v}\) and -\(\hat{c}_\mathrm{d}\) are the viscous damping and drag parameters during acceleration, and -\(\check{c}_\mathrm{v}\) and \(\check{c}_\mathrm{d}\) those during deceleration. - The rigid-body equations of motion are: \[ @@ -41,9 +35,7 @@ \dot{\mathbf{p}} &= \mathbf{v}, \\ \dot{\mathbf{q}} &= \tfrac{1}{2} \mathbf{q} \otimes \begin{bmatrix} {}^{\mathcal{B}}\boldsymbol{\omega}\\0 \end{bmatrix}, \\ - m\dot{\mathbf{v}} &= m\mathbf{g} - + \mathbf{R}\,{}^{\mathcal{B}}\mathbf{f}_\mathrm{t} - + \mathbf{R}\,{}^{\mathcal{B}}\mathbf{f}_\mathrm{a}, \\ + m\dot{\mathbf{v}} &= \mathbf{f}_\Sigma, \\ \mathbf{J}\,{}^{\mathcal{B}}\dot{\boldsymbol{\omega}} &= {}^{\mathcal{B}}\mathbf{t}_\Sigma - {}^{\mathcal{B}}\boldsymbol{\omega} @@ -51,56 +43,83 @@ \end{aligned} \] -where \(\mathbf{p}\) and \(\mathbf{v}\) are the position and velocity, \(\mathbf{q}\) is the -orientation as a scalar-last quaternion, \({}^{\mathcal{B}}\boldsymbol{\omega}\) is the angular -velocity, \({}^{\mathcal{B}}(\cdot)\) denotes the body frame, \(\otimes\) is the quaternion product, -\(m\) is the mass, \(\mathbf{J}\) is the inertia matrix, \(\mathbf{g}\) is the gravity vector, -\(\mathbf{R} = {}^{\mathcal{I}}\mathbf{R}_{\mathcal{B}}(\mathbf{q})\) is the rotation from body to -world frame, and the forces and torques are: +where the total force and torque are: \[ \begin{aligned} - {}^{\mathcal{B}}\mathbf{f}_\mathrm{t} &= - \mathbf{e}_\mathrm{z} \textstyle\sum_{i=1}^{4} f_{\mathrm{m},i}, \\ - {}^{\mathcal{B}}\mathbf{f}_\mathrm{a} &= \mathbf{C}_\mathrm{a}\,\mathbf{R}^{\top}\mathbf{v}, \\ - {}^{\mathcal{B}}\mathbf{t}_\Sigma &= - {}^{\mathcal{B}}\mathbf{t}_\mathrm{t} + \mathbf{f}_\Sigma &= \mathbf{f}_\mathrm{g} + + \mathbf{R}\,{}^{\mathcal{B}}\mathbf{f}_\mathrm{t} + + \mathbf{R}\,{}^{\mathcal{B}}\mathbf{f}_\mathrm{a}, \\ + {}^{\mathcal{B}}\mathbf{t}_\Sigma &= {}^{\mathcal{B}}\mathbf{t}_\mathrm{t} + {}^{\mathcal{B}}\mathbf{t}_\mathrm{d} + {}^{\mathcal{B}}\mathbf{t}_\mathrm{g} - + {}^{\mathcal{B}}\mathbf{t}_\mathrm{r}, + + {}^{\mathcal{B}}\mathbf{t}_\mathrm{r}. \end{aligned} \] -with: +The individual terms are defined as: \[ \begin{aligned} + \mathbf{f}_\mathrm{g} &= m\mathbf{g}, \\ + {}^{\mathcal{B}}\mathbf{f}_\mathrm{t} &= + \mathbf{e}_\mathrm{z} \textstyle\sum_{i=1}^{4} f_{\mathrm{m},i}, \\ + {}^{\mathcal{B}}\mathbf{f}_\mathrm{a} &= \mathbf{C}_\mathrm{a}\,{}^{\mathcal{B}}\mathbf{v}, \\ {}^{\mathcal{B}}\mathbf{t}_\mathrm{t} &= - l - \begin{bmatrix}1&0&0\\0&1&0\\0&0&0\end{bmatrix} - \mathbf{M}\,\mathbf{f}_\mathrm{m}, \\ + l\,\mathrm{diag}(1, 1, 0)\,\mathbf{M}\,\mathbf{f}_\mathrm{m}, \\ {}^{\mathcal{B}}\mathbf{t}_\mathrm{d} &= - \begin{bmatrix}0&0&0\\0&0&0\\0&0&1\end{bmatrix} - \mathbf{M}\,\mathbf{t}_\mathrm{m}, \\ + \mathrm{diag}(0, 0, 1)\,\mathbf{M}\,\mathbf{t}_\mathrm{m}, \\ {}^{\mathcal{B}}\mathbf{t}_\mathrm{g} &= J_\mathrm{p} \left({}^{\mathcal{B}}\boldsymbol{\omega} \times \mathbf{e}_\mathrm{z}\right) \mathbf{e}_\mathrm{z}^{\top} \mathbf{M}\,\boldsymbol{\Omega}, \\ {}^{\mathcal{B}}\mathbf{t}_\mathrm{r} &= J_\mathrm{p}\,\mathbf{e}_\mathrm{z}\, - \mathbf{e}_\mathrm{z}^{\top} \mathbf{M}\,\dot{\boldsymbol{\Omega}}, + \mathbf{e}_\mathrm{z}^{\top} \mathbf{M}\,\dot{\boldsymbol{\Omega}}. \end{aligned} \] -where \({}^{\mathcal{B}}\mathbf{f}_\mathrm{t}\) and \({}^{\mathcal{B}}\mathbf{f}_\mathrm{a}\) are -the thrust and aerodynamic drag force, \({}^{\mathcal{B}}\mathbf{t}_\mathrm{t}\), -\({}^{\mathcal{B}}\mathbf{t}_\mathrm{d}\), \({}^{\mathcal{B}}\mathbf{t}_\mathrm{g}\), and -\({}^{\mathcal{B}}\mathbf{t}_\mathrm{r}\) are the thrust, drag, gyroscopic, and reaction torque, -\(\mathbf{e}_\mathrm{z}\) is the unit vector in z direction, \(\mathbf{C}_\mathrm{a}\) is the -body-frame drag matrix, \(l\) is the distance of the motors to the body axes, \(\mathbf{M}\) is -the \(3\times 4\) mixing matrix, \(J_\mathrm{p}\) is the combined inertia of one propeller and its -motor, and \(\mathbf{f}_\mathrm{m}\), \(\mathbf{t}_\mathrm{m}\), and \(\boldsymbol{\Omega}\) stack -the four motor thrusts, torques, and rotor speeds. The gyroscopic and reaction torques convert -\(\boldsymbol{\Omega}\) and \(\dot{\boldsymbol{\Omega}}\) to rad/s internally, so \(J_\mathrm{p}\) -is in SI units. +The gyroscopic and reaction torques convert \(\boldsymbol{\Omega}\) and +\(\dot{\boldsymbol{\Omega}}\) to rad/s. + +| Variable | Name | Description | +| --- | --- | --- | +| \(\boldsymbol{\Omega}\), \(\Omega_i\) | `rotor_vel` | Rotor speeds in RPM | +| \(\boldsymbol{\Omega}_\mathrm{cmd}\) | `cmd` | Commanded rotor speeds in RPM | +| \(\mathbf{f}_\mathrm{m}\), \(f_{\mathrm{m},i}\) | | Motor thrusts | +| \(\mathbf{t}_\mathrm{m}\), \(t_{\mathrm{m},i}\) | | Motor drag torques | +| \(\mathbf{p}\) | `pos` | Position in m | +| \(\mathbf{q}\) | `quat` | Orientation as a scalar-last quaternion | +| \(\mathbf{v}\) | `vel` | Velocity in m/s | +| \({}^{\mathcal{B}}\boldsymbol{\omega}\) | `ang_vel` | Angular velocity in rad/s | +| \(\mathbf{f}_\Sigma\) | | Total force | +| \({}^{\mathcal{B}}\mathbf{t}_\Sigma\) | | Total torque | +| \(\mathbf{f}_\mathrm{g}\) | | Gravitational force | +| \({}^{\mathcal{B}}\mathbf{f}_\mathrm{t}\) | | Thrust force | +| \({}^{\mathcal{B}}\mathbf{f}_\mathrm{a}\) | | Aerodynamic drag force | +| \({}^{\mathcal{B}}\mathbf{t}_\mathrm{t}\) | | Thrust torque | +| \({}^{\mathcal{B}}\mathbf{t}_\mathrm{d}\) | | Aerodynamic counter torque of the propellers | +| \({}^{\mathcal{B}}\mathbf{t}_\mathrm{g}\) | | Gyroscopic torque | +| \({}^{\mathcal{B}}\mathbf{t}_\mathrm{r}\) | | Reaction torque | +| \(\mathbf{R}\) | | Rotation from body to world frame | +| \(\mathbf{e}_\mathrm{z}\) | | Unit vector in z direction | +| \({}^{\mathcal{B}}(\cdot)\) | | Quantity in the body frame, world frame if unmarked | +| \(\otimes\) | | Quaternion product | +| \(\mathrm{diag}(\cdot)\) | | Diagonal matrix | + +| Parameter | Name | Description | +| --- | --- | --- | +| \(\hat{c}_\mathrm{v}\) | `rotor_dyn_coef` | Viscous damping on spin-up, entry 0 | +| \(\hat{c}_\mathrm{d}\) | `rotor_dyn_coef` | Drag on spin-up, entry 1 | +| \(\check{c}_\mathrm{v}\) | `rotor_dyn_coef` | Viscous damping on spin-down, entry 2 | +| \(\check{c}_\mathrm{d}\) | `rotor_dyn_coef` | Drag on spin-down, entry 3 | +| \(k_{\mathrm{f},0}, k_{\mathrm{f},1}, k_{\mathrm{f},2}\) | `rpm2thrust` | Thrust curve | +| \(k_{\mathrm{t},0}, k_{\mathrm{t},1}, k_{\mathrm{t},2}\) | `rpm2torque` | Torque curve | +| \(m\) | `mass` | Mass in kg | +| \(\mathbf{J}\) | `J` | Inertia matrix in kg m² | +| \(\mathbf{g}\) | `gravity_vec` | Gravity vector in m/s² | +| \(l\) | `L` | Distance of the motors to the body axes in m | +| \(\mathbf{M}\) | `mixing_matrix` | Mixing matrix of motor placement and spin direction | +| \(J_\mathrm{p}\) | `prop_inertia` | Combined inertia of one propeller and its motor in kg m² | +| \(\mathbf{C}_\mathrm{a}\) | `drag_matrix` | Drag coefficients in matrix form in N/(m/s) | """ from crazyflow.dynamics.first_principles.dynamics import ( diff --git a/crazyflow/dynamics/first_principles/dynamics.py b/crazyflow/dynamics/first_principles/dynamics.py index 62824fa1..9a1167dc 100644 --- a/crazyflow/dynamics/first_principles/dynamics.py +++ b/crazyflow/dynamics/first_principles/dynamics.py @@ -79,19 +79,20 @@ def dynamics( mass: Mass of the drone (kg). L: Distance of the motors to the body axes (m). Shared (1,) or one value per motor (4,). - prop_inertia: Inertia of the propellers in z direction (kg m^2). Shared (1,) or one value - per motor (4,). + prop_inertia: Combined inertia of one propeller and its motor (kg m^2). Shared (1,) or one + value per motor (4,). gravity_vec: Gravity vector (m/s^2). We assume the gravity vector points downwards, e.g. [0, 0, -9.81]. J: Inertia matrix (kg m^2). J_inv: Inverse inertia matrix (1/kg m^2). - rpm2thrust: Propeller force constants (N min^2). Shared (1, 3) or one curve per motor - (4, 3). - rpm2torque: Propeller torque constants (Nm min^2). Shared (1, 3) or one curve per motor - (4, 3). - mixing_matrix: Mixing matrix denoting the turn direction of the motors (4x3). - drag_matrix: Drag matrix containing the linear drag coefficients (3x3). - rotor_dyn_coef: Rotor dynamics coefficients. Shared (1, 4) or one set per motor (4, 4). + rpm2thrust: Thrust curve coefficients [k_f0, k_f1, k_f2] for rotor speeds in RPM. Shared + (1, 3) or one curve per motor (4, 3). + rpm2torque: Torque curve coefficients [k_t0, k_t1, k_t2] for rotor speeds in RPM. Shared + (1, 3) or one curve per motor (4, 3). + mixing_matrix: Mixing matrix of motor placement and spin direction (3x4). + drag_matrix: Drag coefficients in matrix form (N/(m/s), 3x3). + rotor_dyn_coef: Rotor dynamics coefficients, viscous damping and drag on spin-up followed by + viscous damping and drag on spin-down. Shared (1, 4) or one set per motor (4, 4). Note: All array parameters accept leading batch axes (N, M) to vary per world and per drone. @@ -206,20 +207,22 @@ def symbolic_dynamics( mass: Drone mass in kg. L: Distance of the motors to the body axes in meters, shared ``(1,)`` or one value per motor ``(4,)``. - prop_inertia: Moment of inertia of the propellers about their spin axis in kg m², shared - ``(1,)`` or one value per motor ``(4,)``. + prop_inertia: Combined inertia of one propeller and its motor in kg m², shared ``(1,)`` or + one value per motor ``(4,)``. gravity_vec: Gravity vector, shape ``(3,)``. J: Inertia matrix, shape ``(3, 3)``. J_inv: Inverse inertia matrix, shape ``(3, 3)``. - rpm2thrust: Polynomial coefficients ``[a, b, c]`` for the thrust curve - ``f = a + b * rpm + c * rpm²``, shared ``(1, 3)`` or one curve per motor ``(4, 3)``. - rpm2torque: Polynomial coefficients ``[a, b, c]`` for the drag-torque curve - ``τ = a + b * rpm + c * rpm²``, shared ``(1, 3)`` or one curve per motor ``(4, 3)``. - mixing_matrix: Matrix of shape ``(3, 4)`` mapping per-motor forces to body torques. - rotor_dyn_coef: Four rotor dynamics coefficients ``[k_acc1, k_acc2, k_dec1, k_dec2]`` used - in the piecewise-linear spin-up/down model, shared ``(1, 4)`` or one set per motor + rpm2thrust: Thrust curve coefficients ``[k_f0, k_f1, k_f2]`` with + ``f = k_f0 + k_f1 * rpm + k_f2 * rpm²``, shared ``(1, 3)`` or one curve per motor + ``(4, 3)``. + rpm2torque: Torque curve coefficients ``[k_t0, k_t1, k_t2]`` with + ``t = k_t0 + k_t1 * rpm + k_t2 * rpm²``, shared ``(1, 3)`` or one curve per motor + ``(4, 3)``. + mixing_matrix: Mixing matrix of motor placement and spin direction, shape ``(3, 4)``. + rotor_dyn_coef: Rotor dynamics coefficients, viscous damping and drag on spin-up followed by + viscous damping and drag on spin-down, shared ``(1, 4)`` or one set per motor ``(4, 4)``. - drag_matrix: Diagonal ``(3, 3)`` matrix of linear drag coefficients. + drag_matrix: Drag coefficients in matrix form in N/(m/s), shape ``(3, 3)``. Returns: Tuple ``(X_dot, X, U, Y)`` of CasADi ``MX`` expressions: @@ -318,9 +321,9 @@ class Params: mass: Array = field(metadata={CORE_NDIM_KEY: 1}) # (1,) """Mass of the drone.""" L: Array = field(metadata={CORE_NDIM_KEY: 1}) # (1,) - """Arm length of the drone. One shared value, or one value per motor with shape (4,).""" + """Distance of the motors to the body axes. One shared value, or one per motor (4,).""" prop_inertia: Array = field(metadata={CORE_NDIM_KEY: 1}) # (1,) - """Inertia of the propellers. One shared value, or one value per motor with shape (4,).""" + """Inertia of one propeller and its motor. One shared value, or one per motor (4,).""" gravity_vec: Array = field(metadata={CORE_NDIM_KEY: 1}) # (3,) """Gravity vector of the drone.""" J: Array = field(metadata={CORE_NDIM_KEY: 2}) # (3, 3) @@ -328,15 +331,15 @@ class Params: J_inv: Array = field(metadata={CORE_NDIM_KEY: 2}) # (3, 3) """Inverse of the inertia matrix of the drone.""" rpm2thrust: Array = field(metadata={CORE_NDIM_KEY: 2}) # (1, 3) - """Force constants of the drone. One shared curve, or one curve per motor with shape (4, 3).""" + """Thrust curve coefficients. One shared curve, or one curve per motor with shape (4, 3).""" rpm2torque: Array = field(metadata={CORE_NDIM_KEY: 2}) # (1, 3) - """Torque constants of the drone. One shared curve, or one curve per motor with shape (4, 3).""" + """Torque curve coefficients. One shared curve, or one curve per motor with shape (4, 3).""" mixing_matrix: Array = field(metadata={CORE_NDIM_KEY: 2}) # (3, 4) - """Mixing matrix of the drone.""" + """Mixing matrix of motor placement and spin direction.""" drag_matrix: Array = field(metadata={CORE_NDIM_KEY: 2}) # (3, 3) - """Drag matrix of the drone.""" + """Drag coefficients in matrix form.""" rotor_dyn_coef: Array = field(metadata={CORE_NDIM_KEY: 2}) # (1, 4) - """Rotor speed dynamics coefficients of the drone. One shared set, or one per motor (4, 4).""" + """Rotor dynamics coefficients. One shared set, or one per motor (4, 4).""" @staticmethod def create(drone: str, device: Device) -> Params: diff --git a/crazyflow/dynamics/first_principles/params.toml b/crazyflow/dynamics/first_principles/params.toml index c185599e..e3e10ac1 100644 --- a/crazyflow/dynamics/first_principles/params.toml +++ b/crazyflow/dynamics/first_principles/params.toml @@ -22,7 +22,7 @@ # [-1.0, 1.0, 1.0, -1.0], # [-1.0, 1.0, -1.0, 1.0] # ] -# drag_matrix = [ # 1/s, zero disables drag +# drag_matrix = [ # N/(m/s), zero disables drag # [0.0, 0.0, 0.0], # [0.0, 0.0, 0.0], # [0.0, 0.0, 0.0] diff --git a/crazyflow/dynamics/so_rpy/__init__.py b/crazyflow/dynamics/so_rpy/__init__.py index 3399e661..7f95b3fd 100644 --- a/crazyflow/dynamics/so_rpy/__init__.py +++ b/crazyflow/dynamics/so_rpy/__init__.py @@ -7,9 +7,9 @@ \[ \begin{aligned} \dot{\mathbf{p}} &= \mathbf{v}, \\ - m\dot{\mathbf{v}} &= m\mathbf{g} - + (c_\mathrm{acc} + c_\mathrm{f} f_{\Sigma,\mathrm{cmd}}) - \,\mathbf{R}\,\mathbf{e}_\mathrm{z}, \\ + m\dot{\mathbf{v}} &= \mathbf{f}_\mathrm{g} + + \mathbf{R}\,\mathbf{e}_\mathrm{z} + (c_\mathrm{acc} + c_\mathrm{f} f_{\Sigma,\mathrm{cmd}}), \\ \ddot{\boldsymbol{\Psi}} &= \boldsymbol{c}_{\boldsymbol{\Psi},1}\,\boldsymbol{\Psi} + \boldsymbol{c}_{\boldsymbol{\Psi},2}\,\dot{\boldsymbol{\Psi}} @@ -17,15 +17,7 @@ \end{aligned} \] -where \(\mathbf{p}\) and \(\mathbf{v}\) are the position and velocity, \(m\) is the mass, -\(\mathbf{g}\) is the gravity vector, \(\mathbf{e}_\mathrm{z}\) is the unit vector in z direction, -\(\mathbf{R} = {}^{\mathcal{I}}\mathbf{R}_{\mathcal{B}}(\boldsymbol{\Psi})\) is the rotation from -body to world frame, \(\boldsymbol{\Psi} = [\phi,\theta,\psi]^{\top}\) holds the roll, pitch, and -yaw angles with rates \(\dot{\boldsymbol{\Psi}}\), and \(f_{\Sigma,\mathrm{cmd}}\) and -\(\boldsymbol{\Psi}_\mathrm{cmd}\) are the commanded collective thrust and attitude. The thrust -offset \(c_\mathrm{acc}\), the thrust scaling coefficient \(c_\mathrm{f}\), and the rotational -coefficients \(\boldsymbol{c}_{\boldsymbol{\Psi},1}\), \(\boldsymbol{c}_{\boldsymbol{\Psi},2}\), and -\(\boldsymbol{c}_{\boldsymbol{\Psi},3}\) are identified from flight data. +where \(\mathbf{f}_\mathrm{g} = m\mathbf{g}\). !!! note This is the native Euler-angle form, matching @@ -37,6 +29,29 @@ Integrating from \({}^{\mathcal{B}}\boldsymbol{\omega}\) rather than \(\dot{\boldsymbol{\Psi}}\) makes the discrete trajectory differ slightly from integrating the Euler state directly. The difference, however, is negligible at our default frequency of 500 Hz. + +| Variable | Name | Description | +| --- | --- | --- | +| \(\mathbf{p}\) | `pos` | Position in m | +| \(\mathbf{v}\) | `vel` | Velocity in m/s | +| \(\boldsymbol{\Psi} = [\phi,\theta,\psi]^{\top}\) | | Roll, pitch, and yaw in rad, from `quat` | +| \(\dot{\boldsymbol{\Psi}}\) | | Roll, pitch, and yaw rates in rad/s, from `ang_vel` | +| \(\boldsymbol{\Psi}_\mathrm{cmd}\) | `cmd[:3]` | Commanded roll, pitch, and yaw in rad | +| \(f_{\Sigma,\mathrm{cmd}}\) | `cmd[3]` | Commanded collective thrust in N | +| \(\mathbf{f}_\mathrm{g}\) | | Gravitational force | +| \(\mathbf{R}\) | | Rotation from body to world frame | +| \(\mathbf{e}_\mathrm{z}\) | | Unit vector in z direction | +| \({}^{\mathcal{B}}(\cdot)\) | | Quantity in the body frame, world frame if unmarked | + +| Parameter | Name | Description | +| --- | --- | --- | +| \(m\) | `mass` | Mass in kg | +| \(\mathbf{g}\) | `gravity_vec` | Gravity vector in m/s² | +| \(c_\mathrm{acc}\) | `acc_coef` | Thrust offset | +| \(c_\mathrm{f}\) | `cmd_f_coef` | Thrust scaling coefficient | +| \(\boldsymbol{c}_{\boldsymbol{\Psi},1}\) | `rpy_coef` | Rotational dynamics coefficients | +| \(\boldsymbol{c}_{\boldsymbol{\Psi},2}\) | `rpy_rates_coef` | Rotational dynamics coefficients | +| \(\boldsymbol{c}_{\boldsymbol{\Psi},3}\) | `cmd_rpy_coef` | Rotational dynamics coefficients | """ from crazyflow.dynamics.so_rpy.dynamics import ( diff --git a/crazyflow/dynamics/so_rpy/dynamics.py b/crazyflow/dynamics/so_rpy/dynamics.py index b031909f..9e85b6ac 100644 --- a/crazyflow/dynamics/so_rpy/dynamics.py +++ b/crazyflow/dynamics/so_rpy/dynamics.py @@ -74,11 +74,12 @@ def dynamics( [0, 0, -9.81]. J: Inertia matrix (kg m^2). J_inv: Inverse inertia matrix (1/kg m^2). - acc_coef: Coefficient for the acceleration (1/s^2). - cmd_f_coef: Coefficient for the collective thrust (N/rad^2). - rpy_coef: Coefficient for the roll pitch yaw dynamics (1/s). - rpy_rates_coef: Coefficient for the roll pitch yaw rates dynamics (1/s^2). - cmd_rpy_coef: Coefficient for the roll pitch yaw command dynamics (1/s). + acc_coef: Thrust offset (N). + cmd_f_coef: Thrust scaling coefficient. + rpy_coef: Rotational dynamics coefficients of the roll, pitch, and yaw angles (1/s^2). + rpy_rates_coef: Rotational dynamics coefficients of the roll, pitch, and yaw rates (1/s). + cmd_rpy_coef: Rotational dynamics coefficients of the commanded roll, pitch, and yaw + (1/s^2). Returns: The derivatives (pos_dot, quat_dot, vel_dot, ang_vel_dot). @@ -184,11 +185,14 @@ def symbolic_dynamics( gravity_vec: Gravity vector, shape ``(3,)``. J: Inertia matrix, shape ``(3, 3)``. J_inv: Inverse inertia matrix, shape ``(3, 3)``. - acc_coef: Scalar acceleration offset coefficient. - cmd_f_coef: Collective-thrust-to-acceleration coefficient. - rpy_coef: RPY state feedback coefficient, shape ``(3,)``. - rpy_rates_coef: RPY-rate feedback coefficient, shape ``(3,)``. - cmd_rpy_coef: RPY command feedforward coefficient, shape ``(3,)``. + acc_coef: Thrust offset in N. + cmd_f_coef: Thrust scaling coefficient. + rpy_coef: Rotational dynamics coefficients of the roll, pitch, and yaw angles in 1/s², shape + ``(3,)``. + rpy_rates_coef: Rotational dynamics coefficients of the roll, pitch, and yaw rates in 1/s, + shape ``(3,)``. + cmd_rpy_coef: Rotational dynamics coefficients of the commanded roll, pitch, and yaw in + 1/s², shape ``(3,)``. Returns: Tuple ``(X_dot, X, U, Y)`` of CasADi ``MX`` expressions: @@ -274,11 +278,14 @@ def symbolic_dynamics_euler( Args: mass: Drone mass in kg. gravity_vec: Gravity vector, shape ``(3,)``. - acc_coef: Scalar acceleration offset coefficient. - cmd_f_coef: Collective-thrust-to-acceleration coefficient. - rpy_coef: RPY state feedback coefficient, shape ``(3,)``. - rpy_rates_coef: RPY-rate feedback coefficient, shape ``(3,)``. - cmd_rpy_coef: RPY command feedforward coefficient, shape ``(3,)``. + acc_coef: Thrust offset in N. + cmd_f_coef: Thrust scaling coefficient. + rpy_coef: Rotational dynamics coefficients of the roll, pitch, and yaw angles in 1/s², shape + ``(3,)``. + rpy_rates_coef: Rotational dynamics coefficients of the roll, pitch, and yaw rates in 1/s, + shape ``(3,)``. + cmd_rpy_coef: Rotational dynamics coefficients of the commanded roll, pitch, and yaw in + 1/s², shape ``(3,)``. Returns: Tuple ``(X_dot, X, U, Y)`` of CasADi ``MX`` expressions: @@ -328,19 +335,19 @@ class Params: """Inverse of the inertia matrix of the drone.""" acc_coef: Array = field(metadata={CORE_NDIM_KEY: 1}) # (1,) - """Coefficient for the acceleration.""" + """Thrust offset.""" cmd_f_coef: Array = field(metadata={CORE_NDIM_KEY: 1}) # (1,) - """Coefficient for the collective thrust.""" + """Thrust scaling coefficient.""" rpy_coef: Array = field(metadata={CORE_NDIM_KEY: 1}) # (3,) - """Coefficient for the roll pitch yaw dynamics.""" + """Rotational dynamics coefficients of the roll, pitch, and yaw angles.""" rpy_rates_coef: Array = field(metadata={CORE_NDIM_KEY: 1}) # (3,) - """Coefficient for the roll pitch yaw rates dynamics.""" + """Rotational dynamics coefficients of the roll, pitch, and yaw rates.""" cmd_rpy_coef: Array = field(metadata={CORE_NDIM_KEY: 1}) # (3,) - """Coefficient for the roll pitch yaw command dynamics.""" + """Rotational dynamics coefficients of the commanded roll, pitch, and yaw.""" @staticmethod def create(drone: str, device: Device) -> Params: diff --git a/crazyflow/dynamics/so_rpy_rotor/__init__.py b/crazyflow/dynamics/so_rpy_rotor/__init__.py index 301abfe8..61bdcbb8 100644 --- a/crazyflow/dynamics/so_rpy_rotor/__init__.py +++ b/crazyflow/dynamics/so_rpy_rotor/__init__.py @@ -8,10 +8,10 @@ \[ \begin{aligned} - \dot{f}_\Sigma &= c_\tau (f_{\Sigma,\mathrm{cmd}} - f_\Sigma), \\ \dot{\mathbf{p}} &= \mathbf{v}, \\ - m\dot{\mathbf{v}} &= m\mathbf{g} - + (c_\mathrm{acc} + c_\mathrm{f} f_\Sigma)\,\mathbf{R}\,\mathbf{e}_\mathrm{z}, \\ + m\dot{\mathbf{v}} &= \mathbf{f}_\mathrm{g} + + \mathbf{R}\,\mathbf{e}_\mathrm{z} (c_\mathrm{acc} + c_\mathrm{f} f_\Sigma), \\ + \dot{f}_\Sigma &= c_\tau (f_{\Sigma,\mathrm{cmd}} - f_\Sigma), \\ \ddot{\boldsymbol{\Psi}} &= \boldsymbol{c}_{\boldsymbol{\Psi},1}\,\boldsymbol{\Psi} + \boldsymbol{c}_{\boldsymbol{\Psi},2}\,\dot{\boldsymbol{\Psi}} @@ -19,20 +19,34 @@ \end{aligned} \] -where \(f_\Sigma\) is the collective thrust, \(\mathbf{p}\) and \(\mathbf{v}\) are the position and -velocity, \(m\) is the mass, \(\mathbf{g}\) is the gravity vector, \(\mathbf{e}_\mathrm{z}\) is the -unit vector in z direction, \(\mathbf{R} = -{}^{\mathcal{I}}\mathbf{R}_{\mathcal{B}}(\boldsymbol{\Psi})\) is the rotation from body to world -frame, \(\boldsymbol{\Psi} = [\phi,\theta,\psi]^{\top}\) holds the roll, pitch, and yaw angles with -rates \(\dot{\boldsymbol{\Psi}}\), and \(f_{\Sigma,\mathrm{cmd}}\) and -\(\boldsymbol{\Psi}_\mathrm{cmd}\) are the commanded collective thrust and attitude. The thrust -dynamics coefficient \(c_\tau\), the thrust offset \(c_\mathrm{acc}\), the thrust scaling -coefficient \(c_\mathrm{f}\), and the rotational coefficients -\(\boldsymbol{c}_{\boldsymbol{\Psi},1}\), \(\boldsymbol{c}_{\boldsymbol{\Psi},2}\), and -\(\boldsymbol{c}_{\boldsymbol{\Psi},3}\) are identified from flight data. +where \(\mathbf{f}_\mathrm{g} = m\mathbf{g}\). This is the native Euler-angle form. For how the simulation integrates this state in quaternion + angular velocity coordinates, see [so_rpy][crazyflow.dynamics.so_rpy]. + +| Variable | Name | Description | +| --- | --- | --- | +| \(\mathbf{p}\) | `pos` | Position in m | +| \(\mathbf{v}\) | `vel` | Velocity in m/s | +| \(\boldsymbol{\Psi} = [\phi,\theta,\psi]^{\top}\) | | Roll, pitch, and yaw in rad, from `quat` | +| \(\dot{\boldsymbol{\Psi}}\) | | Roll, pitch, and yaw rates in rad/s, from `ang_vel` | +| \(f_\Sigma\) | `rotor_vel[0]` | Collective thrust in N | +| \(\boldsymbol{\Psi}_\mathrm{cmd}\) | `cmd[:3]` | Commanded roll, pitch, and yaw in rad | +| \(f_{\Sigma,\mathrm{cmd}}\) | `cmd[3]` | Commanded collective thrust in N | +| \(\mathbf{f}_\mathrm{g}\) | | Gravitational force | +| \(\mathbf{R}\) | | Rotation from body to world frame | +| \(\mathbf{e}_\mathrm{z}\) | | Unit vector in z direction | + +| Parameter | Name | Description | +| --- | --- | --- | +| \(m\) | `mass` | Mass in kg | +| \(\mathbf{g}\) | `gravity_vec` | Gravity vector in m/s² | +| \(c_\tau\) | `thrust_dyn_coef` | Thrust dynamics coefficient in 1/s | +| \(c_\mathrm{acc}\) | `acc_coef` | Thrust offset | +| \(c_\mathrm{f}\) | `cmd_f_coef` | Thrust scaling coefficient | +| \(\boldsymbol{c}_{\boldsymbol{\Psi},1}\) | `rpy_coef` | Rotational dynamics coefficients | +| \(\boldsymbol{c}_{\boldsymbol{\Psi},2}\) | `rpy_rates_coef` | Rotational dynamics coefficients | +| \(\boldsymbol{c}_{\boldsymbol{\Psi},3}\) | `cmd_rpy_coef` | Rotational dynamics coefficients | """ from crazyflow.dynamics.so_rpy_rotor.dynamics import ( diff --git a/crazyflow/dynamics/so_rpy_rotor/dynamics.py b/crazyflow/dynamics/so_rpy_rotor/dynamics.py index 4c1c95c9..a69fa2f6 100644 --- a/crazyflow/dynamics/so_rpy_rotor/dynamics.py +++ b/crazyflow/dynamics/so_rpy_rotor/dynamics.py @@ -81,12 +81,13 @@ def dynamics( [0, 0, -9.81]. J: Inertia matrix (kg m^2). J_inv: Inverse inertia matrix (1/kg m^2). - thrust_dyn_coef: Coefficient for the rotor dynamics (1/s). - acc_coef: Coefficient for the acceleration (1/s^2). - cmd_f_coef: Coefficient for the collective thrust (N/rad^2). - rpy_coef: Coefficient for the roll pitch yaw dynamics (1/s). - rpy_rates_coef: Coefficient for the roll pitch yaw rates dynamics (1/s^2). - cmd_rpy_coef: Coefficient for the roll pitch yaw command dynamics (1/s). + thrust_dyn_coef: Thrust dynamics coefficient (1/s). + acc_coef: Thrust offset (N). + cmd_f_coef: Thrust scaling coefficient. + rpy_coef: Rotational dynamics coefficients of the roll, pitch, and yaw angles (1/s^2). + rpy_rates_coef: Rotational dynamics coefficients of the roll, pitch, and yaw rates (1/s). + cmd_rpy_coef: Rotational dynamics coefficients of the commanded roll, pitch, and yaw + (1/s^2). Returns: The derivatives (pos_dot, quat_dot, vel_dot, ang_vel_dot, rotor_vel_dot). @@ -209,12 +210,15 @@ def symbolic_dynamics( gravity_vec: Gravity vector, shape ``(3,)``. J: Inertia matrix, shape ``(3, 3)``. J_inv: Inverse inertia matrix, shape ``(3, 3)``. - thrust_dyn_coef: First-order thrust lag coefficient (1/s). - acc_coef: Scalar acceleration offset coefficient. - cmd_f_coef: Collective-thrust-to-acceleration coefficient. - rpy_coef: RPY state feedback coefficient, shape ``(3,)``. - rpy_rates_coef: RPY-rate feedback coefficient, shape ``(3,)``. - cmd_rpy_coef: RPY command feedforward coefficient, shape ``(3,)``. + thrust_dyn_coef: Thrust dynamics coefficient in 1/s. + acc_coef: Thrust offset in N. + cmd_f_coef: Thrust scaling coefficient. + rpy_coef: Rotational dynamics coefficients of the roll, pitch, and yaw angles in 1/s², shape + ``(3,)``. + rpy_rates_coef: Rotational dynamics coefficients of the roll, pitch, and yaw rates in 1/s, + shape ``(3,)``. + cmd_rpy_coef: Rotational dynamics coefficients of the commanded roll, pitch, and yaw in + 1/s², shape ``(3,)``. Returns: Tuple ``(X_dot, X, U, Y)`` of CasADi ``MX`` expressions: @@ -320,12 +324,15 @@ def symbolic_dynamics_euler( gravity_vec: Gravity vector, shape ``(3,)``. J: Inertia matrix, shape ``(3, 3)``. J_inv: Inverse inertia matrix, shape ``(3, 3)``. - thrust_dyn_coef: First-order thrust lag coefficient (1/s). - acc_coef: Scalar acceleration offset coefficient. - cmd_f_coef: Collective-thrust-to-acceleration coefficient. - rpy_coef: RPY state feedback coefficient, shape ``(3,)``. - rpy_rates_coef: RPY-rate feedback coefficient, shape ``(3,)``. - cmd_rpy_coef: RPY command feedforward coefficient, shape ``(3,)``. + thrust_dyn_coef: Thrust dynamics coefficient in 1/s. + acc_coef: Thrust offset in N. + cmd_f_coef: Thrust scaling coefficient. + rpy_coef: Rotational dynamics coefficients of the roll, pitch, and yaw angles in 1/s², shape + ``(3,)``. + rpy_rates_coef: Rotational dynamics coefficients of the roll, pitch, and yaw rates in 1/s, + shape ``(3,)``. + cmd_rpy_coef: Rotational dynamics coefficients of the commanded roll, pitch, and yaw in + 1/s², shape ``(3,)``. Returns: Tuple ``(X_dot, X, U, Y)`` of CasADi ``MX`` expressions: @@ -383,17 +390,17 @@ class Params: J_inv: Array = field(metadata={CORE_NDIM_KEY: 2}) # (3, 3) """Inverse of the inertia matrix of the drone.""" thrust_dyn_coef: Array = field(metadata={CORE_NDIM_KEY: 1}) # (1,) - """Rotor coefficient of the drone.""" + """Thrust dynamics coefficient.""" acc_coef: Array = field(metadata={CORE_NDIM_KEY: 1}) # (1,) - """Acceleration coefficient of the drone.""" + """Thrust offset.""" cmd_f_coef: Array = field(metadata={CORE_NDIM_KEY: 1}) # (1,) - """Collective thrust coefficient of the drone.""" + """Thrust scaling coefficient.""" rpy_coef: Array = field(metadata={CORE_NDIM_KEY: 1}) # (3,) - """Roll pitch yaw coefficient of the drone.""" + """Rotational dynamics coefficients of the roll, pitch, and yaw angles.""" rpy_rates_coef: Array = field(metadata={CORE_NDIM_KEY: 1}) # (3,) - """Roll pitch yaw rates coefficient of the drone.""" + """Rotational dynamics coefficients of the roll, pitch, and yaw rates.""" cmd_rpy_coef: Array = field(metadata={CORE_NDIM_KEY: 1}) # (3,) - """Roll pitch yaw command coefficient of the drone.""" + """Rotational dynamics coefficients of the commanded roll, pitch, and yaw.""" @staticmethod def create(drone: str, device: Device) -> Params: diff --git a/crazyflow/dynamics/so_rpy_rotor_drag/__init__.py b/crazyflow/dynamics/so_rpy_rotor_drag/__init__.py index 8545571e..231ab21e 100644 --- a/crazyflow/dynamics/so_rpy_rotor_drag/__init__.py +++ b/crazyflow/dynamics/so_rpy_rotor_drag/__init__.py @@ -8,11 +8,11 @@ \[ \begin{aligned} - \dot{f}_\Sigma &= c_\tau (f_{\Sigma,\mathrm{cmd}} - f_\Sigma), \\ \dot{\mathbf{p}} &= \mathbf{v}, \\ - m\dot{\mathbf{v}} &= m\mathbf{g} - + (c_\mathrm{acc} + c_\mathrm{f} f_\Sigma)\,\mathbf{R}\,\mathbf{e}_\mathrm{z} - + \mathbf{R}\,\mathbf{C}_\mathrm{a}\,\mathbf{R}^{\top}\mathbf{v}, \\ + m\dot{\mathbf{v}} &= \mathbf{f}_\mathrm{g} + + \mathbf{R}\,\mathbf{e}_\mathrm{z} (c_\mathrm{acc} + c_\mathrm{f} f_\Sigma) + + \mathbf{R}\,{}^{\mathcal{B}}\mathbf{f}_\mathrm{a}, \\ + \dot{f}_\Sigma &= c_\tau (f_{\Sigma,\mathrm{cmd}} - f_\Sigma), \\ \ddot{\boldsymbol{\Psi}} &= \boldsymbol{c}_{\boldsymbol{\Psi},1}\,\boldsymbol{\Psi} + \boldsymbol{c}_{\boldsymbol{\Psi},2}\,\dot{\boldsymbol{\Psi}} @@ -20,21 +20,38 @@ \end{aligned} \] -where \(f_\Sigma\) is the collective thrust, \(\mathbf{p}\) and \(\mathbf{v}\) are the position and -velocity, \(m\) is the mass, \(\mathbf{g}\) is the gravity vector, \(\mathbf{e}_\mathrm{z}\) is the -unit vector in z direction, \(\mathbf{R} = -{}^{\mathcal{I}}\mathbf{R}_{\mathcal{B}}(\boldsymbol{\Psi})\) is the rotation from body to world -frame, \(\boldsymbol{\Psi} = [\phi,\theta,\psi]^{\top}\) holds the roll, pitch, and yaw angles with -rates \(\dot{\boldsymbol{\Psi}}\), and \(f_{\Sigma,\mathrm{cmd}}\) and -\(\boldsymbol{\Psi}_\mathrm{cmd}\) are the commanded collective thrust and attitude. The thrust -dynamics coefficient \(c_\tau\), the diagonal body-frame drag matrix \(\mathbf{C}_\mathrm{a}\), the -thrust offset \(c_\mathrm{acc}\), the thrust scaling coefficient \(c_\mathrm{f}\), and the -rotational coefficients \(\boldsymbol{c}_{\boldsymbol{\Psi},1}\), -\(\boldsymbol{c}_{\boldsymbol{\Psi},2}\), and \(\boldsymbol{c}_{\boldsymbol{\Psi},3}\) are -identified from flight data. +where \(\mathbf{f}_\mathrm{g} = m\mathbf{g}\) and +\({}^{\mathcal{B}}\mathbf{f}_\mathrm{a} = \mathbf{C}_\mathrm{a}\,{}^{\mathcal{B}}\mathbf{v}\). This is the native Euler-angle form. For how the simulation integrates this state in quaternion + angular velocity coordinates, see [so_rpy][crazyflow.dynamics.so_rpy]. + +| Variable | Name | Description | +| --- | --- | --- | +| \(\mathbf{p}\) | `pos` | Position in m | +| \(\mathbf{v}\) | `vel` | Velocity in m/s | +| \(\boldsymbol{\Psi} = [\phi,\theta,\psi]^{\top}\) | | Roll, pitch, and yaw in rad, from `quat` | +| \(\dot{\boldsymbol{\Psi}}\) | | Roll, pitch, and yaw rates in rad/s, from `ang_vel` | +| \(f_\Sigma\) | `rotor_vel[0]` | Collective thrust in N | +| \(\boldsymbol{\Psi}_\mathrm{cmd}\) | `cmd[:3]` | Commanded roll, pitch, and yaw in rad | +| \(f_{\Sigma,\mathrm{cmd}}\) | `cmd[3]` | Commanded collective thrust in N | +| \(\mathbf{f}_\mathrm{g}\) | | Gravitational force | +| \({}^{\mathcal{B}}\mathbf{f}_\mathrm{a}\) | | Aerodynamic drag force | +| \(\mathbf{R}\) | | Rotation from body to world frame | +| \(\mathbf{e}_\mathrm{z}\) | | Unit vector in z direction | +| \({}^{\mathcal{B}}(\cdot)\) | | Quantity in the body frame, world frame if unmarked | + +| Parameter | Name | Description | +| --- | --- | --- | +| \(m\) | `mass` | Mass in kg | +| \(\mathbf{g}\) | `gravity_vec` | Gravity vector in m/s² | +| \(c_\tau\) | `thrust_dyn_coef` | Thrust dynamics coefficient in 1/s | +| \(\mathbf{C}_\mathrm{a}\) | `drag_matrix` | Drag coefficients in matrix form in N/(m/s) | +| \(c_\mathrm{acc}\) | `acc_coef` | Thrust offset | +| \(c_\mathrm{f}\) | `cmd_f_coef` | Thrust scaling coefficient | +| \(\boldsymbol{c}_{\boldsymbol{\Psi},1}\) | `rpy_coef` | Rotational dynamics coefficients | +| \(\boldsymbol{c}_{\boldsymbol{\Psi},2}\) | `rpy_rates_coef` | Rotational dynamics coefficients | +| \(\boldsymbol{c}_{\boldsymbol{\Psi},3}\) | `cmd_rpy_coef` | Rotational dynamics coefficients | """ from crazyflow.dynamics.so_rpy_rotor_drag.dynamics import ( diff --git a/crazyflow/dynamics/so_rpy_rotor_drag/dynamics.py b/crazyflow/dynamics/so_rpy_rotor_drag/dynamics.py index 206d0afb..cf9cc516 100644 --- a/crazyflow/dynamics/so_rpy_rotor_drag/dynamics.py +++ b/crazyflow/dynamics/so_rpy_rotor_drag/dynamics.py @@ -93,13 +93,14 @@ def dynamics( [0, 0, -9.81]. J: Inertia matrix (kg m^2). J_inv: Inverse inertia matrix (1/kg m^2). - thrust_dyn_coef: Coefficient for the rotor dynamics (1/s). - acc_coef: Coefficient for the acceleration (1/s^2). - cmd_f_coef: Coefficient for the collective thrust (N/rad^2). - rpy_coef: Coefficient for the roll pitch yaw dynamics (1/s). - rpy_rates_coef: Coefficient for the roll pitch yaw rates dynamics (1/s^2). - cmd_rpy_coef: Coefficient for the roll pitch yaw command dynamics (1/s). - drag_matrix: Coefficient matrix for the linear drag (1/s). + thrust_dyn_coef: Thrust dynamics coefficient (1/s). + acc_coef: Thrust offset (N). + cmd_f_coef: Thrust scaling coefficient. + rpy_coef: Rotational dynamics coefficients of the roll, pitch, and yaw angles (1/s^2). + rpy_rates_coef: Rotational dynamics coefficients of the roll, pitch, and yaw rates (1/s). + cmd_rpy_coef: Rotational dynamics coefficients of the commanded roll, pitch, and yaw + (1/s^2). + drag_matrix: Drag coefficients in matrix form (N/(m/s)). Returns: The derivatives (pos_dot, quat_dot, vel_dot, ang_vel_dot, rotor_vel_dot). @@ -231,14 +232,16 @@ def symbolic_dynamics( gravity_vec: Gravity vector, shape ``(3,)``. J: Inertia matrix, shape ``(3, 3)``. J_inv: Inverse inertia matrix, shape ``(3, 3)``. - thrust_dyn_coef: First-order thrust lag coefficient (1/s). - acc_coef: Scalar acceleration offset coefficient. - cmd_f_coef: Collective-thrust-to-acceleration coefficient. - rpy_coef: RPY state feedback coefficient, shape ``(3,)``. - rpy_rates_coef: RPY-rate feedback coefficient, shape ``(3,)``. - cmd_rpy_coef: RPY command feedforward coefficient, shape ``(3,)``. - drag_matrix: Diagonal ``(3, 3)`` matrix of linear drag coefficients applied in the body - frame. + thrust_dyn_coef: Thrust dynamics coefficient in 1/s. + acc_coef: Thrust offset in N. + cmd_f_coef: Thrust scaling coefficient. + rpy_coef: Rotational dynamics coefficients of the roll, pitch, and yaw angles in 1/s², shape + ``(3,)``. + rpy_rates_coef: Rotational dynamics coefficients of the roll, pitch, and yaw rates in 1/s, + shape ``(3,)``. + cmd_rpy_coef: Rotational dynamics coefficients of the commanded roll, pitch, and yaw in + 1/s², shape ``(3,)``. + drag_matrix: Drag coefficients in matrix form in N/(m/s), diagonal ``(3, 3)``. Returns: Tuple ``(X_dot, X, U, Y)`` of CasADi ``MX`` expressions: @@ -348,14 +351,16 @@ def symbolic_dynamics_euler( gravity_vec: Gravity vector, shape ``(3,)``. J: Inertia matrix, shape ``(3, 3)``. J_inv: Inverse inertia matrix, shape ``(3, 3)``. - thrust_dyn_coef: First-order thrust lag coefficient (1/s). - acc_coef: Scalar acceleration offset coefficient. - cmd_f_coef: Collective-thrust-to-acceleration coefficient. - rpy_coef: RPY state feedback coefficient, shape ``(3,)``. - rpy_rates_coef: RPY-rate feedback coefficient, shape ``(3,)``. - cmd_rpy_coef: RPY command feedforward coefficient, shape ``(3,)``. - drag_matrix: Diagonal ``(3, 3)`` matrix of linear drag coefficients applied in the body - frame. + thrust_dyn_coef: Thrust dynamics coefficient in 1/s. + acc_coef: Thrust offset in N. + cmd_f_coef: Thrust scaling coefficient. + rpy_coef: Rotational dynamics coefficients of the roll, pitch, and yaw angles in 1/s², shape + ``(3,)``. + rpy_rates_coef: Rotational dynamics coefficients of the roll, pitch, and yaw rates in 1/s, + shape ``(3,)``. + cmd_rpy_coef: Rotational dynamics coefficients of the commanded roll, pitch, and yaw in + 1/s², shape ``(3,)``. + drag_matrix: Drag coefficients in matrix form in N/(m/s), diagonal ``(3, 3)``. Returns: Tuple ``(X_dot, X, U, Y)`` of CasADi ``MX`` expressions: @@ -417,19 +422,19 @@ class Params: J_inv: Array = field(metadata={CORE_NDIM_KEY: 2}) # (3, 3) """Inverse of the inertia matrix of the drone.""" thrust_dyn_coef: Array = field(metadata={CORE_NDIM_KEY: 1}) # (1,) - """Rotor coefficient of the drone.""" + """Thrust dynamics coefficient.""" acc_coef: Array = field(metadata={CORE_NDIM_KEY: 1}) # (1,) - """Acceleration coefficient of the drone.""" + """Thrust offset.""" cmd_f_coef: Array = field(metadata={CORE_NDIM_KEY: 1}) # (1,) - """Collective thrust coefficient of the drone.""" + """Thrust scaling coefficient.""" rpy_coef: Array = field(metadata={CORE_NDIM_KEY: 1}) # (3,) - """Roll pitch yaw coefficient of the drone.""" + """Rotational dynamics coefficients of the roll, pitch, and yaw angles.""" rpy_rates_coef: Array = field(metadata={CORE_NDIM_KEY: 1}) # (3,) - """Roll pitch yaw rates coefficient of the drone.""" + """Rotational dynamics coefficients of the roll, pitch, and yaw rates.""" cmd_rpy_coef: Array = field(metadata={CORE_NDIM_KEY: 1}) # (3,) - """Roll pitch yaw command coefficient of the drone.""" + """Rotational dynamics coefficients of the commanded roll, pitch, and yaw.""" drag_matrix: Array = field(metadata={CORE_NDIM_KEY: 2}) # (3, 3) - """Linear drag coefficient matrix of the drone.""" + """Drag coefficients in matrix form.""" @staticmethod def create(drone: str, device: Device) -> Params: diff --git a/crazyflow/dynamics/so_rpy_rotor_drag/params.toml b/crazyflow/dynamics/so_rpy_rotor_drag/params.toml index f4097b4d..b1013e6e 100644 --- a/crazyflow/dynamics/so_rpy_rotor_drag/params.toml +++ b/crazyflow/dynamics/so_rpy_rotor_drag/params.toml @@ -15,7 +15,7 @@ # rpy_rates_coef = [0.0, 0.0, 0.0] # cmd_rpy_coef = [0.0, 0.0, 0.0] # thrust_dyn_coef = 0.0 # 1/s -# drag_matrix = [ # 1/s +# drag_matrix = [ # N/(m/s) # [0.0, 0.0, 0.0], # [0.0, 0.0, 0.0], # [0.0, 0.0, 0.0]