diff --git a/include/Utils/Matrix3d.h b/include/Utils/Matrix3d.h index 60175e6f..e3617995 100644 --- a/include/Utils/Matrix3d.h +++ b/include/Utils/Matrix3d.h @@ -9,6 +9,8 @@ namespace BIORBD_NAMESPACE { namespace utils { +class String; + /// /// \brief A wrapper for the Eigen::MatrixXd /// @@ -73,6 +75,12 @@ class BIORBD_API Matrix3d : public RigidBodyDynamics::Math::Matrix3d utils::Matrix3d orthoNormalize() const; #endif + /// + /// \brief Creates a base 3d matrix from a euler sequence of exactly 3 elements + /// \return The base matrix + /// + static utils::Matrix3d fromEulerSequence(const utils::String& seq); + #ifdef BIORBD_USE_CASADI_MATH /// diff --git a/include/Utils/Quaternion.h b/include/Utils/Quaternion.h index 1d8fdd2f..5b5f57da 100644 --- a/include/Utils/Quaternion.h +++ b/include/Utils/Quaternion.h @@ -1,8 +1,6 @@ #ifndef BIORBD_UTILS_QUATERNION_H #define BIORBD_UTILS_QUATERNION_H -#include -#include "Utils/Vector3d.h" #include "Utils/Scalar.h" #include "biorbdConfig.h" @@ -11,6 +9,7 @@ namespace BIORBD_NAMESPACE namespace utils { class Vector3d; +class Matrix3d; class Vector; class RotoTrans; class Rotation; @@ -303,32 +302,46 @@ class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d const Vector3d &vec) const; /// - /// \brief Converts a 3d angular velocity vector + /// \brief Converts a 3d angular velocity vector into a 4d derivative of the components of the quaternion /// \param omega the angular velocity /// \return a 4d vector containing the derivatives of the 4 components of the quaternion corresponding to omega /// - /// Converts a 3d angular velocity vector into a 4d derivative of - /// the components of the quaternion - /// - Quaternion omegaToQDot( + Quaternion omegaToQuatDot( const Vector3d& omega) const; + /// + /// \brief Generate the velocity matrix which allows to go from/to euler angles to/from omega(body velocity) + /// \param euler the Euler angles + /// \param seq the Euler angles sequence + /// \return a 3d matrix + /// + Matrix3d velocityMatrix ( + const Vector3d &euler, + const String& seq); + /// /// \brief Converts a 3d angular velocity vector /// \param eulerDot the Euler angle rates /// \param euler the Euler angles /// \param seq the Euler angles sequence - /// - /// Converts a 3d angular velocity vector expressed in terms of euler - /// angles rate into the 3d angular velocity vector expressed in the fixed - /// parent frame. See - /// https://davidbrown3.github.io/2017-07-25/EulerAngles/ - /// for correct equations. + /// \return a 3d vector of the body angular velocity /// Vector3d eulerDotToOmega( const Vector3d &eulerDot, const Vector3d &euler, const String& seq); + + /// + /// \brief converts a 3d vector of the body angular velocity (omega) into a 3d vector of the euler angles rate. + /// \param euler the Euler angles + /// \param w the body velocity (omega) + /// \param seq the Euler angles sequence + /// \return a 3d vector of the euler angles rate + /// + Vector3d omegaToEulerDot( + const Vector3d &euler, + const Vector3d &w, + const String& seq); /// /// \brief Return the time derivative of the quaterion diff --git a/src/Utils/Matrix3d.cpp b/src/Utils/Matrix3d.cpp index 173ddd23..66e421cf 100644 --- a/src/Utils/Matrix3d.cpp +++ b/src/Utils/Matrix3d.cpp @@ -2,6 +2,7 @@ #include "Utils/Matrix3d.h" #include "Utils/Vector.h" +#include "Utils/Error.h" using namespace BIORBD_NAMESPACE; @@ -56,6 +57,32 @@ utils::Matrix3d utils::Matrix3d::orthoNormalize() const } #endif +utils::Matrix3d utils::Matrix3d::fromEulerSequence( + const utils::String& seq) +{ + utils::Error::check(seq.length() == 3, "The angle sequence should be exactly 3."); + + utils::Matrix3d baseUnitMatrix(utils::Matrix3d::Zero()); + for (int i = 0; i < seq.length(); i++) { + int indexSeq; + if (seq[i] == 'x') { + indexSeq = 0; + } + else if (seq[i] == 'y') { + indexSeq = 1; + } + else if (seq[i] == 'z') { + indexSeq = 2; + } + else { + utils::Error::raise("Angle sequence must be composed of x, y, and/or z."); + } + baseUnitMatrix(i, indexSeq) = 1; + } + return baseUnitMatrix; +} + + #ifdef BIORBD_USE_CASADI_MATH utils::Matrix3d::Matrix3d( diff --git a/src/Utils/Quaternion.cpp b/src/Utils/Quaternion.cpp index 578b2aec..413a2323 100644 --- a/src/Utils/Quaternion.cpp +++ b/src/Utils/Quaternion.cpp @@ -2,6 +2,7 @@ #include "Utils/Quaternion.h" #include "Utils/Vector3d.h" +#include "Utils/Matrix3d.h" #include "Utils/Vector.h" #include "Utils/RotoTrans.h" #include "Utils/Error.h" @@ -311,7 +312,7 @@ utils::Vector3d utils::Quaternion::rotate( } #include -utils::Quaternion utils::Quaternion::omegaToQDot( +utils::Quaternion utils::Quaternion::omegaToQuatDot( const utils::Vector3d &omega) const { RigidBodyDynamics::Math::MatrixNd m(4, 3); @@ -331,30 +332,45 @@ utils::Quaternion utils::Quaternion::omegaToQDot( return utils::Quaternion(0.5 * m * omega, this->m_Kstab); } +utils::Matrix3d utils::Quaternion::velocityMatrix( + const utils::Vector3d &euler, + const utils::String& seq) +{ + utils::Matrix3d baseMatrix = utils::Matrix3d::fromEulerSequence(seq); + utils::Vector3d rot1(euler[0], 0, 0); + utils::Vector3d rot2(euler[0], euler[1], 0); + utils::Matrix3d RotMat1 = utils::Rotation::fromEulerAngles(rot1, seq); + utils::Matrix3d RotMat2 = utils::Rotation::fromEulerAngles(rot2, seq); + + utils::Matrix3d result; + result.block(0, 0, 3, 1) = baseMatrix.block(0, 0, 3, 1); + result.block(0, 1, 3, 1) = RotMat1 * baseMatrix.block(0, 1, 3, 1); + result.block(0, 2, 3, 1) = RotMat2 * baseMatrix.block(0, 2, 3, 1); + return result; +} + + utils::Vector3d utils::Quaternion::eulerDotToOmega( - const utils::Vector3d &eulerDot, const utils::Vector3d &euler, + const utils::Vector3d &eulerDot, const utils::String& seq) { + utils:Matrix3d velocity_matrix = velocityMatrix(euler, seq); + utils::Vector3d w = velocity_matrix * eulerDot; - utils::Vector3d w; - utils::Scalar dph, dth, dps, ph, th, ps; - dph = eulerDot[0]; - dth = eulerDot[1]; - dps = eulerDot[2]; - ph = euler[0]; - th = euler[1]; - ps = euler[2]; - if (!seq.compare("xyz")) { // xyz - w[0] = dph*std::cos(th)*std::cos(ps) + dth*std::sin(ps); - w[1] = dth*std::cos(ps) - dph*std::cos(th)*std::sin(ps); - w[2] = dph*std::sin(th) + dps; - } else { - utils::Error::raise("Angle sequence is either nor implemented or not recognized"); - } return w; } - + +utils::Vector3d utils::Quaternion::omegaToEulerDot( + const utils::Vector3d &euler, + const utils::Vector3d &w, + const utils::String& seq) +{ + utils:Matrix3d velocity_matrix = velocityMatrix(euler, seq).inverse(); + utils::Vector3d eulerDot = velocity_matrix * w; + return eulerDot; +} + void utils::Quaternion::derivate( const utils::Vector &w) { diff --git a/test/test_utils.cpp b/test/test_utils.cpp index 6ac0ca42..f270783c 100644 --- a/test/test_utils.cpp +++ b/test/test_utils.cpp @@ -1205,7 +1205,7 @@ TEST(Quaternion, otherOperations) EXPECT_NEAR(vecZ, 582, requiredPrecision); } { utils::Quaternion q(2, 3, 4, 5, 6); - utils::Quaternion qdot(q.omegaToQDot(utils::Vector3d(7, 8, 9))); + utils::Quaternion qdot(q.omegaToQuatDot(utils::Vector3d(7, 8, 9))); SCALAR_TO_DOUBLE(qdotW, qdot.w()); SCALAR_TO_DOUBLE(qdotX, qdot.x()); @@ -1263,9 +1263,18 @@ TEST(Quaternion, velocities) SCALAR_TO_DOUBLE(w0, w[0]); SCALAR_TO_DOUBLE(w1, w[1]); SCALAR_TO_DOUBLE(w2, w[2]); - EXPECT_NEAR(w0, 0.52227744876434945, requiredPrecision); - EXPECT_NEAR(w1, 0.36181645351259678, requiredPrecision); - EXPECT_NEAR(w2, 0.67946773231802449, requiredPrecision); + EXPECT_NEAR(w0, 0.243827661581261, requiredPrecision); + EXPECT_NEAR(w1, 0.0816881748534787, requiredPrecision); + EXPECT_NEAR(w2, 0.320375788494034, requiredPrecision); + + utils::Vector3d eulDot(q.omegaToEulerDot(eR,w,"xyz")); + + SCALAR_TO_DOUBLE(eulDot0, eulDot[0]); + SCALAR_TO_DOUBLE(eulDot1, eulDot[1]); + SCALAR_TO_DOUBLE(eulDot2, eulDot[2]); + EXPECT_NEAR(eulDot0, 0.1, requiredPrecision); + EXPECT_NEAR(eulDot1, 0.2, requiredPrecision); + EXPECT_NEAR(eulDot2, 0.3, requiredPrecision); } }