From 68a0414622cb2d2734fca2131881f864ba685110 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 7 Mar 2022 17:18:56 +0100 Subject: [PATCH] Quaternion: rename function to rotate vectors --- src/lib/matrix/matrix/Quaternion.hpp | 4 ++-- src/lib/matrix/test/attitude.cpp | 16 ++++++++-------- src/lib/matrix/test/dual.cpp | 2 +- .../attitude_estimator_q_main.cpp | 16 ++++++++-------- .../commander/accelerometer_calibration.cpp | 2 +- src/modules/uuv_pos_control/uuv_pos_control.cpp | 4 ++-- src/systemcmds/tests/test_mathlib.cpp | 10 +++++----- 7 files changed, 27 insertions(+), 27 deletions(-) diff --git a/src/lib/matrix/matrix/Quaternion.hpp b/src/lib/matrix/matrix/Quaternion.hpp index dfe5860b9a..ccd65d7e3e 100644 --- a/src/lib/matrix/matrix/Quaternion.hpp +++ b/src/lib/matrix/matrix/Quaternion.hpp @@ -485,7 +485,7 @@ public: * @param vec vector to rotate in frame 1 (typically body frame) * @return rotated vector in frame 2 (typically reference frame) */ - Vector3 conjugate(const Vector3 &vec) const + Vector3 rotateVector(const Vector3 &vec) const { const Quaternion &q = *this; Quaternion v(Type(0), vec(0), vec(1), vec(2)); @@ -502,7 +502,7 @@ public: * @param vec vector to rotate in frame 2 (typically reference frame) * @return rotated vector in frame 1 (typically body frame) */ - Vector3 conjugate_inversed(const Vector3 &vec) const + Vector3 rotateVectorInverse(const Vector3 &vec) const { const Quaternion &q = *this; Quaternion v(Type(0), vec(0), vec(1), vec(2)); diff --git a/src/lib/matrix/test/attitude.cpp b/src/lib/matrix/test/attitude.cpp index 1d4a7f1868..84961e827a 100644 --- a/src/lib/matrix/test/attitude.cpp +++ b/src/lib/matrix/test/attitude.cpp @@ -57,27 +57,27 @@ int main() // quaternion ctor: vector to vector // identity test Quatf quat_v(v, v); - TEST(isEqual(quat_v.conjugate(v), v)); + TEST(isEqual(quat_v.rotateVector(v), v)); // random test (vector norm can not be preserved with a pure rotation) Vector3f v1(-80.1f, 1.5f, -6.89f); quat_v = Quatf(v1, v); - TEST(isEqual(quat_v.conjugate(v1).normalized() * v.norm(), v)); + TEST(isEqual(quat_v.rotateVector(v1).normalized() * v.norm(), v)); // special 180 degree case 1 v1 = Vector3f(0.f, 1.f, 1.f); quat_v = Quatf(v1, -v1); - TEST(isEqual(quat_v.conjugate(v1), -v1)); + TEST(isEqual(quat_v.rotateVector(v1), -v1)); // special 180 degree case 2 v1 = Vector3f(1.f, 2.f, 0.f); quat_v = Quatf(v1, -v1); - TEST(isEqual(quat_v.conjugate(v1), -v1)); + TEST(isEqual(quat_v.rotateVector(v1), -v1)); // special 180 degree case 3 v1 = Vector3f(0.f, 0.f, 1.f); quat_v = Quatf(v1, -v1); - TEST(isEqual(quat_v.conjugate(v1), -v1)); + TEST(isEqual(quat_v.rotateVector(v1), -v1)); // special 180 degree case 4 v1 = Vector3f(1.f, 1.f, 1.f); quat_v = Quatf(v1, -v1); - TEST(isEqual(quat_v.conjugate(v1), -v1)); + TEST(isEqual(quat_v.rotateVector(v1), -v1)); // quat normalization q.normalize(); @@ -447,8 +447,8 @@ int main() // conjugate v = Vector3f(1.5f, 2.2f, 3.2f); - TEST(isEqual(q.conjugate_inversed(v1), Dcmf(q).T()*v1)); - TEST(isEqual(q.conjugate(v1), Dcmf(q)*v1)); + TEST(isEqual(q.rotateVectorInverse(v1), Dcmf(q).T()*v1)); + TEST(isEqual(q.rotateVector(v1), Dcmf(q)*v1)); AxisAnglef aa_q_init(q); TEST(isEqual(aa_q_init, AxisAnglef(1.0f, 2.0f, 3.0f))); diff --git a/src/lib/matrix/test/dual.cpp b/src/lib/matrix/test/dual.cpp index d92ff72712..d36bb730fe 100644 --- a/src/lib/matrix/test/dual.cpp +++ b/src/lib/matrix/test/dual.cpp @@ -28,7 +28,7 @@ Vector positionError(const Vector &positionState, Scalar dt ) { - return positionMeasurement - (positionState + bodyOrientation.conjugate(velocityStateBody) * dt); + return positionMeasurement - (positionState + bodyOrientation.rotateVector(velocityStateBody) * dt); } int main() diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index a11af425ed..fd2cf70206 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -327,7 +327,7 @@ AttitudeEstimatorQ::Run() if (_vel_prev_t != 0 && lpos.timestamp != _vel_prev_t) { float vel_dt = (lpos.timestamp - _vel_prev_t) / 1e6f; /* calculate acceleration in body frame */ - _pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt); + _pos_acc = _q.rotateVectorInverse((vel - _vel_prev) / vel_dt); } _vel_prev_t = lpos.timestamp; @@ -452,26 +452,26 @@ AttitudeEstimatorQ::update(float dt) if (_param_att_ext_hdg_m.get() == 1) { // Vision heading correction // Project heading to global frame and extract XY component - Vector3f vision_hdg_earth = _q.conjugate(_vision_hdg); + Vector3f vision_hdg_earth = _q.rotateVector(_vision_hdg); float vision_hdg_err = wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0))); // Project correction to body frame - corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -vision_hdg_err)) * _param_att_w_ext_hdg.get(); + corr += _q.rotateVectorInverse(Vector3f(0.0f, 0.0f, -vision_hdg_err)) * _param_att_w_ext_hdg.get(); } if (_param_att_ext_hdg_m.get() == 2) { // Mocap heading correction // Project heading to global frame and extract XY component - Vector3f mocap_hdg_earth = _q.conjugate(_mocap_hdg); + Vector3f mocap_hdg_earth = _q.rotateVector(_mocap_hdg); float mocap_hdg_err = wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0))); // Project correction to body frame - corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -mocap_hdg_err)) * _param_att_w_ext_hdg.get(); + corr += _q.rotateVectorInverse(Vector3f(0.0f, 0.0f, -mocap_hdg_err)) * _param_att_w_ext_hdg.get(); } } if (_param_att_ext_hdg_m.get() == 0 || !_ext_hdg_good) { // Magnetometer correction // Project mag field vector to global frame and extract XY component - Vector3f mag_earth = _q.conjugate(_mag); + Vector3f mag_earth = _q.rotateVector(_mag); float mag_err = wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl); float gainMult = 1.0f; const float fifty_dps = 0.873f; @@ -481,7 +481,7 @@ AttitudeEstimatorQ::update(float dt) } // Project magnetometer correction to body frame - corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -mag_err)) * _param_att_w_mag.get() * gainMult; + corr += _q.rotateVectorInverse(Vector3f(0.0f, 0.0f, -mag_err)) * _param_att_w_mag.get() * gainMult; } _q.normalize(); @@ -489,7 +489,7 @@ AttitudeEstimatorQ::update(float dt) // Accelerometer correction // Project 'k' unit vector of earth frame to body frame - // Vector3f k = _q.conjugate_inversed(Vector3f(0.0f, 0.0f, 1.0f)); + // Vector3f k = _q.rotateVectorInverse(Vector3f(0.0f, 0.0f, 1.0f)); // Optimized version with dropped zeros Vector3f k( 2.0f * (_q(1) * _q(3) - _q(0) * _q(2)), diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index ffa24a1016..700822bbc5 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -561,7 +561,7 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub) // use vehicle_attitude if available const vehicle_attitude_s &att = attitude_sub.get(); const matrix::Quatf q{att.q}; - const Vector3f accel_ref = q.conjugate_inversed(Vector3f{0.f, 0.f, -CONSTANTS_ONE_G}); + const Vector3f accel_ref = q.rotateVectorInverse(Vector3f{0.f, 0.f, -CONSTANTS_ONE_G}); // sanity check angle between acceleration vectors const float angle = AxisAnglef(Quatf(accel_avg, accel_ref)).angle(); diff --git a/src/modules/uuv_pos_control/uuv_pos_control.cpp b/src/modules/uuv_pos_control/uuv_pos_control.cpp index 37f277d7a3..32ecdaa97e 100644 --- a/src/modules/uuv_pos_control/uuv_pos_control.cpp +++ b/src/modules/uuv_pos_control/uuv_pos_control.cpp @@ -122,7 +122,7 @@ void UUVPOSControl::pose_controller_6dof(const float x_pos_des, const float y_po _param_pose_gain_y.get() * (pos_des(1) - vlocal_pos.y) - _param_pose_gain_d_y.get() * vlocal_pos.vy, _param_pose_gain_z.get() * (pos_des(2) - vlocal_pos.z) - _param_pose_gain_d_z.get() * vlocal_pos.vz); - Vector3f rotated_input = q_att.conjugate_inversed(p_control_output);//rotate the coord.sys (from global to body) + Vector3f rotated_input = q_att.rotateVectorInverse(p_control_output);//rotate the coord.sys (from global to body) publish_attitude_setpoint(rotated_input(0), rotated_input(1), @@ -143,7 +143,7 @@ void UUVPOSControl::stabilization_controller_6dof(const float x_pos_des, const f 0, _param_pose_gain_z.get() * (pos_des(2) - vlocal_pos.z)); //potential d controller missing - Vector3f rotated_input = q_att.conjugate_inversed(p_control_output);//rotate the coord.sys (from global to body) + Vector3f rotated_input = q_att.rotateVectorInverse(p_control_output);//rotate the coord.sys (from global to body) publish_attitude_setpoint(rotated_input(0) + x_pos_des, rotated_input(1) + y_pos_des, rotated_input(2), roll_des, pitch_des, yaw_des); diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index de48b49b10..f4442af96e 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -323,7 +323,7 @@ bool MathlibTest::testQuaternionRotate() R = matrix::Eulerf(roll, pitch, yaw); q = matrix::Eulerf(roll, pitch, yaw); vector_r = R * vector; - vector_q = q.conjugate(vector); + vector_q = q.rotateVector(vector); for (int i = 0; i < 3; i++) { ut_assert("matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_r(i) - vector_q(i)) < tol); @@ -336,7 +336,7 @@ bool MathlibTest::testQuaternionRotate() // test some values calculated with matlab tol = 0.0001f; q = matrix::Eulerf(M_PI_2_F, 0.0f, 0.0f); - vector_q = q.conjugate(vector); + vector_q = q.rotateVector(vector); matrix::Vector3f vector_true = {1.00f, -1.00f, 1.00f}; for (unsigned i = 0; i < 3; i++) { @@ -344,7 +344,7 @@ bool MathlibTest::testQuaternionRotate() } q = matrix::Eulerf(0.3f, 0.2f, 0.1f); - vector_q = q.conjugate(vector); + vector_q = q.rotateVector(vector); vector_true = {1.1566, 0.7792, 1.0273}; for (unsigned i = 0; i < 3; i++) { @@ -352,7 +352,7 @@ bool MathlibTest::testQuaternionRotate() } q = matrix::Eulerf(-1.5f, -0.2f, 0.5f); - vector_q = q.conjugate(vector); + vector_q = q.rotateVector(vector); vector_true = {0.5095, 1.4956, -0.7096}; for (unsigned i = 0; i < 3; i++) { @@ -360,7 +360,7 @@ bool MathlibTest::testQuaternionRotate() } q = matrix::Eulerf(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f); - vector_q = q.conjugate(vector); + vector_q = q.rotateVector(vector); vector_true = { -1.3660, 0.3660, 1.0000}; for (unsigned i = 0; i < 3; i++) {