Merge pull request #151 from pariterre/Quaternion

Added Qranges for quaternions

Author: pariterre

examples/forwardDynamicsExample.cpp

@@ -15,7 +15,7 @@
 int main()
 {
     // Load a predefined model
-    biorbd::Model model("pyomecaman.bioMod");
+    biorbd::Model model("/home/pariterre/Programmation/sandbox/biorbd-dev/ModeleBaton_1bras_EpaulesQuat.bioMod");
 
     // Choose a position/velocity/torque to compute dynamics from
     biorbd::rigidbody::GeneralizedCoordinates Q(model);

src/ModelReader.cpp

@@ -148,6 +148,8 @@ void biorbd::Reader::readModelFile(
                         if (rot.compare("q")){
                             // If not a quaternion
                             rotLength = rot.length();
+                        } else {
+                            rotLength = 4;
                         }
                         for (size_t i=0; i<trans.length() + rotLength; ++i){
                             file.read(min);
@@ -163,6 +165,8 @@ void biorbd::Reader::readModelFile(
                         if (rot.compare("q")){
                             // If not a quaternion
                             rotLength = rot.length();
+                        } else {
+                            rotLength = 3;
                         }
                         for (size_t i=0; i<trans.length() + rotLength; ++i){
                             file.read(min);
@@ -178,6 +182,8 @@ void biorbd::Reader::readModelFile(
                         if (rot.compare("q")){
                             // If not a quaternion
                             rotLength = rot.length();
+                        } else {
+                            rotLength = 3;
                         }
                         for (size_t i=0; i<trans.length() + rotLength; ++i){
                             file.read(min);
@@ -285,17 +291,26 @@ void biorbd::Reader::readModelFile(
                     if (rot.compare("q")){
                         // If not a quaternion
                         rotLength = rot.length();
+                    } else {
+                        rotLength = 4;
                     }
                     for (size_t i=0; i<trans.length() + rotLength; ++i){
-                        QRanges.push_back(
-                                    biorbd::utils::Range ());
+                        if (!rot.compare("q") && i>=trans.length()){
+                            QRanges.push_back(
+                                        biorbd::utils::Range (-1, 1));
+                        } else {
+                            QRanges.push_back(
+                                        biorbd::utils::Range ());
+                        }
                     }
                 }
                 if (!isRangeQDotSet){
                     size_t rotLength(0);
                     if (rot.compare("q")){
                         // If not a quaternion
                         rotLength = rot.length();
+                    } else {
+                        rotLength = 3;
                     }
                     for (size_t i=0; i<trans.length() + rotLength; ++i){
                         QDotRanges.push_back(
@@ -307,6 +322,8 @@ void biorbd::Reader::readModelFile(
                     if (rot.compare("q")){
                         // If not a quaternion
                         rotLength = rot.length();
+                    } else {
+                        rotLength = 3;
                     }
                     for (size_t i=0; i<trans.length() + rotLength; ++i){
                         QDDotRanges.push_back(

src/RigidBody/Segment.cpp

@@ -283,19 +283,22 @@ void biorbd::rigidbody::Segment::setDofs(
     }
     biorbd::utils::Error::check(
                 QRanges.size() == 0 ||
-                QRanges.size() == seqT.length() + nRot,
+                QRanges.size() == seqT.length() + nRot ||
+                (QRanges.size() == 4 && m_isQuaternion),
                 "QRanges and number of dof must be equal");
     *m_QRanges = QRanges;
 
     biorbd::utils::Error::check(
                 QDotRanges.size() == 0 ||
-                QDotRanges.size() == seqT.length() + nRot,
+                QDotRanges.size() == seqT.length() + nRot ||
+                (QDotRanges.size() == 3 && m_isQuaternion),
                 "QDotRanges and number of dof must be equal");
     *m_QDotRanges = QDotRanges;
 
     biorbd::utils::Error::check(
                 QDDotRanges.size() == 0 ||
-                QDDotRanges.size() == seqT.length() + nRot,
+                QDDotRanges.size() == seqT.length() + nRot ||
+                (QDDotRanges.size() == 3 && m_isQuaternion),
                 "QDDotRanges and number of dof must be equal");
     *m_QDDotRanges = QDDotRanges;
     setJoints(model);