diff --git a/crazyflow/dynamics/first_principles/__init__.py b/crazyflow/dynamics/first_principles/__init__.py index 62076062..1224ba4f 100644 --- a/crazyflow/dynamics/first_principles/__init__.py +++ b/crazyflow/dynamics/first_principles/__init__.py @@ -1,28 +1,31 @@ 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: +When rotor dynamics are modelled, the rotor speeds evolve as: \[ - 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. + \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} \] -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} - c_1 (\Omega_{\mathrm{cmd},i} - \Omega_i) - + c_2 (\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) - & \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. \] The rigid-body equations of motion are: @@ -32,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} - + R\,{}^{\mathcal{B}}\mathbf{f}_t - + R\,{}^{\mathcal{B}}\mathbf{f}_a, \\ + m\dot{\mathbf{v}} &= \mathbf{f}_\Sigma, \\ \mathbf{J}\,{}^{\mathcal{B}}\dot{\boldsymbol{\omega}} &= {}^{\mathcal{B}}\mathbf{t}_\Sigma - {}^{\mathcal{B}}\boldsymbol{\omega} @@ -42,43 +43,83 @@ \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 the total force and torque 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{t}_\Sigma &= - {}^{\mathcal{B}}\mathbf{t}_t - + {}^{\mathcal{B}}\mathbf{t}_d - + {}^{\mathcal{B}}\mathbf{t}_i, + \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}. \end{aligned} \] -with: +The individual terms are defined as: \[ \begin{aligned} - {}^{\mathcal{B}}\mathbf{t}_t &= - \frac{l}{\sqrt{2}} - \begin{bmatrix}1&0&0\\0&1&0\\0&0&0\end{bmatrix} - M\,\mathbf{f}_p, \\ - {}^{\mathcal{B}}\mathbf{t}_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{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\,\mathrm{diag}(1, 1, 0)\,\mathbf{M}\,\mathbf{f}_\mathrm{m}, \\ + {}^{\mathcal{B}}\mathbf{t}_\mathrm{d} &= + \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}}. \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. +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 38b85f22..9a1167dc 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,20 +78,21 @@ 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,). - prop_inertia: Inertia of the propellers in z direction (kg m^2). 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: 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. @@ -205,22 +205,24 @@ 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,)``. + 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: @@ -319,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) @@ -329,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 0d2fda1b..e3e10ac1 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 @@ -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 c71d4324..7f95b3fd 100644 --- a/crazyflow/dynamics/so_rpy/__init__.py +++ b/crazyflow/dynamics/so_rpy/__init__.py @@ -7,29 +7,51 @@ \[ \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}}, + 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}} + + \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{f}_\mathrm{g} = m\mathbf{g}\). !!! 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. + +| 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 059b8fdc..61bdcbb8 100644 --- a/crazyflow/dynamics/so_rpy_rotor/__init__.py +++ b/crazyflow/dynamics/so_rpy_rotor/__init__.py @@ -1,30 +1,52 @@ 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{\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}}, + 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}} + + \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 \(\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 4bb3ad68..a69fa2f6 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,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_time_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). @@ -109,7 +110,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 +147,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 +158,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 +168,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 +187,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,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_time_coef: First-order thrust lag time constant 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: @@ -240,7 +244,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 +304,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,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_time_coef: First-order thrust lag time constant 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: @@ -349,7 +356,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,18 +389,18 @@ 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,) - """Rotor coefficient of the drone.""" + thrust_dyn_coef: Array = field(metadata={CORE_NDIM_KEY: 1}) # (1,) + """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: @@ -405,7 +412,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 4113992e..231ab21e 100644 --- a/crazyflow/dynamics/so_rpy_rotor_drag/__init__.py +++ b/crazyflow/dynamics/so_rpy_rotor_drag/__init__.py @@ -8,25 +8,50 @@ \[ \begin{aligned} - \dot{F} &= \frac{1}{\tau}(F_{\mathrm{cmd}} - F), \\ \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}}, + 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}} + + \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 \(\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 103ba270..cf9cc516 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,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_time_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). @@ -122,7 +123,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 +161,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 +173,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 +184,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 +208,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,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_time_coef: First-order thrust lag time constant 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: @@ -266,7 +269,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 +330,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,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_time_coef: First-order thrust lag time constant 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: @@ -379,7 +384,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,20 +421,20 @@ 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,) - """Rotor coefficient of the drone.""" + thrust_dyn_coef: Array = field(metadata={CORE_NDIM_KEY: 1}) # (1,) + """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: @@ -441,7 +446,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..b1013e6e 100644 --- a/crazyflow/dynamics/so_rpy_rotor_drag/params.toml +++ b/crazyflow/dynamics/so_rpy_rotor_drag/params.toml @@ -14,8 +14,8 @@ # 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 -# drag_matrix = [ # 1/s +# thrust_dyn_coef = 0.0 # 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] @@ -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/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/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])) 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