Merge pull request #263 from EveCharbie/EulerDot_to_Omega

[RTR] eulerDot - body velocity tranformation

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
 
     ///

include/Utils/Quaternion.h

@@ -1,8 +1,6 @@
 #ifndef BIORBD_UTILS_QUATERNION_H
 #define BIORBD_UTILS_QUATERNION_H
 
-#include <memory>
-#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

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(

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 <iostream>
-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)
 {

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);
     }
 
 }