From 6b7601c0bd31026c28a213acd2baa0518aa67f16 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sat, 31 May 2025 04:17:00 +0300 Subject: [PATCH] Improve vector and quaternion libraries Make the order or basic methods consistent between Vector and Quaternion. Remove `ZYX` from Euler method names as this is standard for robotics. Rename angular rates to rotation vector, which is more correct. Make rotation methods static, to keep the arguments order consistent. Make `Quaternion::fromAxisAngle` accept Vector for axis. Minor fixes. --- flix/cli.ino | 2 +- flix/control.ino | 8 ++-- flix/estimate.ino | 8 ++-- flix/log.ino | 4 +- flix/quaternion.h | 97 +++++++++++++++++++++++++---------------------- flix/vector.h | 22 +++++------ 6 files changed, 74 insertions(+), 67 deletions(-) diff --git a/flix/cli.ino b/flix/cli.ino index 36d281d..a21a746 100644 --- a/flix/cli.ino +++ b/flix/cli.ino @@ -101,7 +101,7 @@ void doCommand(String str, bool echo = false) { print("Loop rate: %.0f\n", loopRate); print("dt: %f\n", dt); } else if (command == "ps") { - Vector a = attitude.toEulerZYX(); + Vector a = attitude.toEuler(); print("roll: %f pitch: %f yaw: %f\n", degrees(a.x), degrees(a.y), degrees(a.z)); } else if (command == "psq") { print("qx: %f qy: %f qz: %f qw: %f\n", attitude.x, attitude.y, attitude.z, attitude.w); diff --git a/flix/control.ino b/flix/control.ino index dd02bbb..0b3ff32 100644 --- a/flix/control.ino +++ b/flix/control.ino @@ -95,7 +95,7 @@ void interpretRC() { } else if (mode == STAB) { yawMode = controls[yawChannel] == 0 ? YAW : YAW_RATE; - attitudeTarget = Quaternion::fromEulerZYX(Vector( + attitudeTarget = Quaternion::fromEuler(Vector( controls[rollChannel] * tiltMax, controls[pitchChannel] * tiltMax, attitudeTarget.getYaw())); @@ -122,10 +122,10 @@ void controlAttitude() { } const Vector up(0, 0, 1); - Vector upActual = attitude.rotateVector(up); - Vector upTarget = attitudeTarget.rotateVector(up); + Vector upActual = Quaternion::rotateVector(up, attitude); + Vector upTarget = Quaternion::rotateVector(up, attitudeTarget); - Vector error = Vector::angularRatesBetweenVectors(upTarget, upActual); + Vector error = Vector::rotationVectorBetween(upTarget, upActual); ratesTarget.x = rollPID.update(error.x, dt); ratesTarget.y = pitchPID.update(error.y, dt); diff --git a/flix/estimate.ino b/flix/estimate.ino index 3d947d2..086fb34 100644 --- a/flix/estimate.ino +++ b/flix/estimate.ino @@ -23,7 +23,7 @@ void applyGyro() { rates = ratesFilter.update(gyro); // apply rates to attitude - attitude = attitude.rotate(Quaternion::fromAngularRates(rates * dt)); + attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(rates * dt)); } void applyAcc() { @@ -34,9 +34,9 @@ void applyAcc() { if (!landed) return; // calculate accelerometer correction - Vector up = attitude.rotateVector(Vector(0, 0, 1)); - Vector correction = Vector::angularRatesBetweenVectors(acc, up) * WEIGHT_ACC; + Vector up = Quaternion::rotateVector(Vector(0, 0, 1), attitude); + Vector correction = Vector::rotationVectorBetween(acc, up) * WEIGHT_ACC; // apply correction - attitude = attitude.rotate(Quaternion::fromAngularRates(correction)); + attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(correction)); } diff --git a/flix/log.ino b/flix/log.ino index 0257da4..1a942ef 100644 --- a/flix/log.ino +++ b/flix/log.ino @@ -41,8 +41,8 @@ float logBuffer[LOG_SIZE][logColumns]; void prepareLogData() { tFloat = t; - attitudeEuler = attitude.toEulerZYX(); - attitudeTargetEuler = attitudeTarget.toEulerZYX(); + attitudeEuler = attitude.toEuler(); + attitudeTargetEuler = attitudeTarget.toEuler(); } void logData() { diff --git a/flix/quaternion.h b/flix/quaternion.h index d8ab190..b61eeb8 100644 --- a/flix/quaternion.h +++ b/flix/quaternion.h @@ -15,22 +15,22 @@ public: Quaternion(float w, float x, float y, float z): w(w), x(x), y(y), z(z) {}; - static Quaternion fromAxisAngle(float a, float b, float c, float angle) { + static Quaternion fromAxisAngle(const Vector& axis, float angle) { float halfAngle = angle * 0.5; float sin2 = sin(halfAngle); float cos2 = cos(halfAngle); - float sinNorm = sin2 / sqrt(a * a + b * b + c * c); - return Quaternion(cos2, a * sinNorm, b * sinNorm, c * sinNorm); + float sinNorm = sin2 / axis.norm(); + return Quaternion(cos2, axis.x * sinNorm, axis.y * sinNorm, axis.z * sinNorm); } - static Quaternion fromAngularRates(const Vector& rates) { - if (rates.zero()) { + static Quaternion fromRotationVector(const Vector& rotation) { + if (rotation.zero()) { return Quaternion(); } - return Quaternion::fromAxisAngle(rates.x, rates.y, rates.z, rates.norm()); + return Quaternion::fromAxisAngle(rotation, rotation.norm()); } - static Quaternion fromEulerZYX(const Vector& euler) { + static Quaternion fromEuler(const Vector& euler) { float cx = cos(euler.x / 2); float cy = cos(euler.y / 2); float cz = cos(euler.z / 2); @@ -60,14 +60,37 @@ public: return ret; } - void toAxisAngle(float& a, float& b, float& c, float& angle) const { - angle = acos(w) * 2; - a = x / sin(angle / 2); - b = y / sin(angle / 2); - c = z / sin(angle / 2); + bool finite() const { + return isfinite(w) && isfinite(x) && isfinite(y) && isfinite(z); } - Vector toEulerZYX() const { + float norm() const { + return sqrt(w * w + x * x + y * y + z * z); + } + + void normalize() { + float n = norm(); + w /= n; + x /= n; + y /= n; + z /= n; + } + + void toAxisAngle(Vector& axis, float& angle) const { + angle = acos(w) * 2; + axis.x = x / sin(angle / 2); + axis.y = y / sin(angle / 2); + axis.z = z / sin(angle / 2); + } + + Vector toRotationVector() const { + float angle; + Vector axis; + toAxisAngle(axis, angle); + return angle * axis; + } + + Vector toEuler() const { // https://github.com/ros/geometry2/blob/589caf083cae9d8fae7effdb910454b4681b9ec1/tf2/include/tf2/impl/utils.h#L87 Vector euler; float sqx = x * x; @@ -112,18 +135,9 @@ public: void setYaw(float yaw) { // TODO: optimize? - Vector euler = toEulerZYX(); + Vector euler = toEuler(); euler.z = yaw; - (*this) = Quaternion::fromEulerZYX(euler); - } - - Quaternion& operator *= (const Quaternion& q) { - Quaternion ret( - w * q.w - x * q.x - y * q.y - z * q.z, - w * q.x + x * q.w + y * q.z - z * q.y, - w * q.y + y * q.w + z * q.x - x * q.z, - w * q.z + z * q.w + x * q.y - y * q.x); - return (*this = ret); + (*this) = Quaternion::fromEuler(euler); } Quaternion operator * (const Quaternion& q) const { @@ -143,18 +157,6 @@ public: -z * normSqInv); } - float norm() const { - return sqrt(w * w + x * x + y * y + z * z); - } - - void normalize() { - float n = norm(); - w /= n; - x /= n; - y /= n; - z /= n; - } - Vector conjugate(const Vector& v) const { Quaternion qv(0, v.x, v.y, v.z); Quaternion res = (*this) * qv * inversed(); @@ -167,22 +169,27 @@ public: return Vector(res.x, res.y, res.z); } - // Rotate vector by quaternion - Vector rotateVector(const Vector& v) const { - return conjugateInversed(v); - } - // Rotate quaternion by quaternion - Quaternion rotate(const Quaternion& q, const bool normalize = true) const { - Quaternion rotated = (*this) * q; + static Quaternion rotate(const Quaternion& a, const Quaternion& b, const bool normalize = true) { + Quaternion rotated = a * b; if (normalize) { rotated.normalize(); } return rotated; } - bool finite() const { - return isfinite(w) && isfinite(x) && isfinite(y) && isfinite(z); + // Rotate vector by quaternion + static Vector rotateVector(const Vector& v, const Quaternion& q) { + return q.conjugateInversed(v); + } + + // Quaternion between two quaternions a and b + static Quaternion between(const Quaternion& a, const Quaternion& b, const bool normalize = true) { + Quaternion q = a * b.inversed(); + if (normalize) { + q.normalize(); + } + return q; } size_t printTo(Print& p) const { diff --git a/flix/vector.h b/flix/vector.h index 2dc4a0a..b2d9a89 100644 --- a/flix/vector.h +++ b/flix/vector.h @@ -13,14 +13,18 @@ public: Vector(float x, float y, float z): x(x), y(y), z(z) {}; - float norm() const { - return sqrt(x * x + y * y + z * z); - } - bool zero() const { return x == 0 && y == 0 && z == 0; } + bool finite() const { + return isfinite(x) && isfinite(y) && isfinite(z); + } + + float norm() const { + return sqrt(x * x + y * y + z * z); + } + void normalize() { float n = norm(); x /= n; @@ -74,10 +78,6 @@ public: return !(*this == b); } - bool finite() const { - return isfinite(x) && isfinite(y) && isfinite(z); - } - static float dot(const Vector& a, const Vector& b) { return a.x * b.x + a.y * b.y + a.z * b.z; } @@ -86,18 +86,18 @@ public: return Vector(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x); } - static float angleBetweenVectors(const Vector& a, const Vector& b) { + static float angleBetween(const Vector& a, const Vector& b) { return acos(constrain(dot(a, b) / (a.norm() * b.norm()), -1, 1)); } - static Vector angularRatesBetweenVectors(const Vector& a, const Vector& b) { + static Vector rotationVectorBetween(const Vector& a, const Vector& b) { Vector direction = cross(a, b); if (direction.zero()) { // vectors are opposite, return any perpendicular vector return cross(a, Vector(1, 0, 0)); } direction.normalize(); - float angle = angleBetweenVectors(a, b); + float angle = angleBetween(a, b); return direction * angle; }