Merge pull request #89 from robotology/recovery

Corrected check on quaternion: now it works either roq or column

mex-wholebodymodel/matlab/utilities/+WBM/+utilities/quat2rotm.m

@@ -1,19 +1,19 @@
 function dcm = quat2rotm(quat)
-    if (size(quat,1) ~= 4)
+    if (length(quat) ~= 4)
         error('quat2rotm: %s', WBM.wbmErrorMsg.WRONG_VEC_DIM);
     end
     dcm = zeros(3,3);
 
-    qnorm = quat.'*quat;
+    qnorm = norm(quat);
     if (qnorm > 1)
-        quat = quat./sqrt(qnorm); % normalize
+        quat = quat./qnorm; % normalize
     end
     % scalar (real) part:
-    q_0 = quat(1,1);
+    q_0 = quat(1);
     % vector (imaginary) part:
-    q_1 = quat(2,1);
-    q_2 = quat(3,1);
-    q_3 = quat(4,1);
+    q_1 = quat(2);
+    q_2 = quat(3);
+    q_3 = quat(4);
 
     %% Compute the Direction Cosine Matrix (DCM) by applying the Euler-Rodrigues Parameterization
     % for efficent computing of the rotation matrix R(s,r) in SO(3) with,
@@ -33,7 +33,7 @@
     %
     % Note: The direct assignment is computationally faster than using directly the formula above. Furthermore, if the above formula
     %       will be used, the matrix multiplications are causing accumulative round-off errors in the main diagonal of the DCM-matrix
-    %       (around ±0.7*1e-03). To be more accurate, it is better to use the derived transformation matrix of the formula.
+    %       (around ??0.7*1e-03). To be more accurate, it is better to use the derived transformation matrix of the formula.
     dcm(1,1) = q_0*q_0 + q_1*q_1 - q_2*q_2 - q_3*q_3;
     dcm(1,2) = 2*(q_1*q_2 - q_0*q_3);
     dcm(1,3) = 2*(q_0*q_2 + q_1*q_3);