diff --git a/crazyflow/dynamics/first_principles/dynamics.py b/crazyflow/dynamics/first_principles/dynamics.py index 6f56ab1..70ee0fd 100644 --- a/crazyflow/dynamics/first_principles/dynamics.py +++ b/crazyflow/dynamics/first_principles/dynamics.py @@ -141,7 +141,7 @@ def dynamics( ) torque_inertia = prop_inertia * xp.stack( [ - -ang_vel[..., 1] * xp.sum(mixing_matrix[..., -1, :] * rotor_vel_rads, axis=-1), + ang_vel[..., 1] * xp.sum(mixing_matrix[..., -1, :] * rotor_vel_rads, axis=-1), -ang_vel[..., 0] * xp.sum(mixing_matrix[..., -1, :] * rotor_vel_rads, axis=-1), xp.sum(mixing_matrix[..., -1, :] * rotor_vel_dot_rads, axis=-1), ], @@ -261,7 +261,7 @@ def symbolic_dynamics( rotor_vel_rads = symbols.rotor_vel * rpm_to_rad rotor_vel_dot_rads = rotor_vel_dot * rpm_to_rad if model_rotor_vel else symbols.rotor_vel * 0.0 torque_inertia = prop_inertia * cs.vertcat( - -symbols.ang_vel[1] * cs.sum(mixing_matrix[-1, :] * rotor_vel_rads), + symbols.ang_vel[1] * cs.sum(mixing_matrix[-1, :] * rotor_vel_rads), -symbols.ang_vel[0] * cs.sum(mixing_matrix[-1, :] * rotor_vel_rads), cs.sum(mixing_matrix[-1, :] * rotor_vel_dot_rads), )