diff --git a/crazyflow/dynamics/first_principles/dynamics.py b/crazyflow/dynamics/first_principles/dynamics.py index 38b85f22..30e0b033 100644 --- a/crazyflow/dynamics/first_principles/dynamics.py +++ b/crazyflow/dynamics/first_principles/dynamics.py @@ -295,7 +295,7 @@ def symbolic_dynamics( # Rotational equation of motion xi = cs.vertcat( - cs.horzcat(0, -symbols.ang_vel.T), cs.horzcat(symbols.ang_vel, -cs.skew(symbols.ang_vel)) + cs.horzcat(-cs.skew(symbols.ang_vel), symbols.ang_vel), cs.horzcat(-symbols.ang_vel.T, 0) ) quat_dot = 0.5 * (xi @ symbols.quat) torques_sum = torques_motor_vec diff --git a/crazyflow/dynamics/so_rpy/dynamics.py b/crazyflow/dynamics/so_rpy/dynamics.py index b031909f..06fd2a8b 100644 --- a/crazyflow/dynamics/so_rpy/dynamics.py +++ b/crazyflow/dynamics/so_rpy/dynamics.py @@ -234,7 +234,7 @@ def symbolic_dynamics( # Rotational equation of motion xi = cs.vertcat( - cs.horzcat(0, -symbols.ang_vel.T), cs.horzcat(symbols.ang_vel, -cs.skew(symbols.ang_vel)) + cs.horzcat(-cs.skew(symbols.ang_vel), symbols.ang_vel), cs.horzcat(-symbols.ang_vel.T, 0) ) quat_dot = 0.5 * (xi @ symbols.quat) ang_vel_dot = rotation.cs_rpy_rates_deriv2ang_vel_deriv( diff --git a/crazyflow/dynamics/so_rpy_rotor/dynamics.py b/crazyflow/dynamics/so_rpy_rotor/dynamics.py index 4bb3ad68..6cbed5eb 100644 --- a/crazyflow/dynamics/so_rpy_rotor/dynamics.py +++ b/crazyflow/dynamics/so_rpy_rotor/dynamics.py @@ -269,7 +269,7 @@ def symbolic_dynamics( # Rotational equation of motion xi = cs.vertcat( - cs.horzcat(0, -symbols.ang_vel.T), cs.horzcat(symbols.ang_vel, -cs.skew(symbols.ang_vel)) + cs.horzcat(-cs.skew(symbols.ang_vel), symbols.ang_vel), cs.horzcat(-symbols.ang_vel.T, 0) ) quat_dot = 0.5 * (xi @ symbols.quat) ang_vel_dot = rotation.cs_rpy_rates_deriv2ang_vel_deriv( diff --git a/crazyflow/dynamics/so_rpy_rotor_drag/dynamics.py b/crazyflow/dynamics/so_rpy_rotor_drag/dynamics.py index 103ba270..f2335976 100644 --- a/crazyflow/dynamics/so_rpy_rotor_drag/dynamics.py +++ b/crazyflow/dynamics/so_rpy_rotor_drag/dynamics.py @@ -296,7 +296,7 @@ def symbolic_dynamics( # Rotational equation of motion xi = cs.vertcat( - cs.horzcat(0, -symbols.ang_vel.T), cs.horzcat(symbols.ang_vel, -cs.skew(symbols.ang_vel)) + cs.horzcat(-cs.skew(symbols.ang_vel), symbols.ang_vel), cs.horzcat(-symbols.ang_vel.T, 0) ) quat_dot = 0.5 * (xi @ symbols.quat) ang_vel_dot = rotation.cs_rpy_rates_deriv2ang_vel_deriv( diff --git a/crazyflow/dynamics/utils/rotation.py b/crazyflow/dynamics/utils/rotation.py index 5366c7b5..408f8f62 100644 --- a/crazyflow/dynamics/utils/rotation.py +++ b/crazyflow/dynamics/utils/rotation.py @@ -16,7 +16,7 @@ def ang_vel2quat_dot(quat: Array, ang_vel: Array) -> Array: - """Calculates the quaternion derivative based on an angular velocity.""" + """Calculates the quaternion derivative based on a body angular velocity.""" xp = array_namespace(quat) # Split angular velocity x = ang_vel[..., 0:1] @@ -31,14 +31,14 @@ def ang_vel2quat_dot(quat: Array, ang_vel: Array) -> Array: ], axis=-2, ) - # First row of Xi - xi1 = xp.concat((xp.zeros_like(x), -ang_vel), axis=-1) - # Second to fourth rows of Xi + # First to third rows of Xi ang_vel_col = xp.expand_dims(ang_vel, axis=-1) # (..., 3, 1) - xi2 = xp.concat((ang_vel_col, -ang_vel_skew), axis=-1) # (..., 3, 4) + xi1 = xp.concat((-ang_vel_skew, ang_vel_col), axis=-1) # (..., 3, 4) + # Fourth row of Xi + xi2 = xp.concat((-ang_vel, xp.zeros_like(x)), axis=-1) # Combine into Xi - xi1_exp = xp.expand_dims(xi1, axis=-2) # (..., 1, 4) - xi = xp.concat((xi1_exp, xi2), axis=-2) # (..., 4, 4) + xi2_exp = xp.expand_dims(xi2, axis=-2) # (..., 1, 4) + xi = xp.concat((xi1, xi2_exp), axis=-2) # (..., 4, 4) # Quaternion derivative quat_exp = xp.expand_dims(quat, axis=-1) # (..., 4, 1) result = 0.5 * xp.matmul(xi, quat_exp) # (..., 4, 1) @@ -199,8 +199,7 @@ def cs_quat2euler(quat: cs.MX, seq: str = "xyz", degrees: bool = False) -> cs.MX CasADi-based solvers. Args: - quat: CasADi ``MX`` column vector of length 4, in scalar-last (xyzw) - convention. + quat: CasADi ``MX`` column vector of length 4 (xyzw). seq: Three-character axis sequence string. Lowercase letters (e.g. ``"xyz"``) denote extrinsic rotations; uppercase (e.g. ``"XYZ"``) denote intrinsic rotations. Consecutive axes must differ. diff --git a/tests/unit/dynamics/test_rotation.py b/tests/unit/dynamics/test_rotation.py index da46999e..cdb1646d 100644 --- a/tests/unit/dynamics/test_rotation.py +++ b/tests/unit/dynamics/test_rotation.py @@ -29,6 +29,25 @@ def create_uniform_ang_vel(N: int = 1000, scale: float = 10) -> NDArray: return np.random.uniform(-scale, scale, size=(N, 3)) +@pytest.mark.unit +def test_ang_vel2quat_dot(): + N = 1000 + rot = R.random(N) + ang_vels = create_uniform_ang_vel(N) + dt = 1e-5 + quats = rot.as_quat() + quat_next = (rot * R.from_rotvec(ang_vels * dt)).as_quat() + quat_prev = (rot * R.from_rotvec(-ang_vels * dt)).as_quat() + # Pick the sign of ±q closest to quats. Using canonical is not sufficient here, as it + # splits quaternions with small signed w differences, but large xyz agreement + quat_next *= np.sign(np.sum(quat_next * quats, axis=-1, keepdims=True)) + quat_prev *= np.sign(np.sum(quat_prev * quats, axis=-1, keepdims=True)) + quat_dot_fd = (quat_next - quat_prev) / (2 * dt) + + quat_dot = rotation.ang_vel2quat_dot(xp.asarray(quats), xp.asarray(ang_vels)) + assert np.allclose(quat_dot, quat_dot_fd, atol=tol), "Quaternion derivative is off." + + @pytest.mark.unit def test_ang_vel2rpy_rates_two_way(): quats = xp.asarray(create_uniform_quats())