diff --git a/+mystica/+rbm/getQuatGivenRotm.m b/+mystica/+rbm/getQuatGivenRotm.m
index ef9c71c..2857486 100644
--- a/+mystica/+rbm/getQuatGivenRotm.m
+++ b/+mystica/+rbm/getQuatGivenRotm.m
@@ -59,3 +59,21 @@
     quat = [qw;qx;qy;qz] / quaternionNorm;
     
 end
+
+%% alternative implementation
+% 
+% function quat = getQuatGivenRotm2(rotm)
+% 
+%     qw = 0.5 * 1                            * sqrt(rotm(1,1) + rotm(2,2) + rotm(3,3) + 1);
+%     qx = 0.5 * sign0(rotm(3,2) - rotm(2,3)) * sqrt(rotm(1,1) - rotm(2,2) - rotm(3,3) + 1);
+%     qy = 0.5 * sign0(rotm(1,3) - rotm(3,1)) * sqrt(rotm(2,2) - rotm(3,3) - rotm(1,1) + 1);
+%     qz = 0.5 * sign0(rotm(2,1) - rotm(1,2)) * sqrt(rotm(3,3) - rotm(1,1) - rotm(2,2) + 1);
+% 
+%     quat = [qw;qx;qy;qz];
+% 
+% end
+% 
+% function s = sign0(x)
+%     s = 2*(x>=0)-1;
+% end
+
diff --git a/+mystica/+rbm/getRotmGivenQuat.m b/+mystica/+rbm/getRotmGivenQuat.m
index 7c38a33..0041d10 100644
--- a/+mystica/+rbm/getRotmGivenQuat.m
+++ b/+mystica/+rbm/getRotmGivenQuat.m
@@ -1,8 +1,8 @@
 function rotm = getRotmGivenQuat(quat)
-    
+
     qw   = quat(1);
     qxyz = quat(2:4);
-    
+
     rotm = eye(3)+2*qw*mystica.utils.skew(qxyz)+2*mystica.utils.skew(qxyz)^2;
-    
+
 end
diff --git a/+mystica/+rbm/invQuat.m b/+mystica/+rbm/invQuat.m
new file mode 100644
index 0000000..5d0b5a7
--- /dev/null
+++ b/+mystica/+rbm/invQuat.m
@@ -0,0 +1,6 @@
+function quat_b_a = invQuat(quat_a_b)
+
+    qw = quat_a_b(1); qxyz = quat_a_b(2:4);
+    quat_b_a = [qw;-qxyz]/(norm(quat_a_b)^2);
+
+end
diff --git a/+mystica/+rbm/logQuat.m b/+mystica/+rbm/logQuat.m
new file mode 100644
index 0000000..1fd98b1
--- /dev/null
+++ b/+mystica/+rbm/logQuat.m
@@ -0,0 +1,19 @@
+function theta = logQuat(quat,input)
+    arguments
+        quat
+        input.selectQuat char {mustBeMember(input.selectQuat,{'positive','minDistance','normal'})} = 'normal'
+    end
+    qw = quat(1); qxyz = quat(2:4);
+    switch input.selectQuat
+        case 'normal'
+            normV = norm(qxyz,2)+eps;
+            theta = 2 * qxyz * atan2(normV,qw)/normV;
+        case 'positive'
+            quat  = -quat*(qw<0)+quat*(qw>=0);
+            theta = mystica.rbm.logQuat(quat,'selectQuat','normal');
+        case 'minDistance'
+            theta1 = mystica.rbm.logQuat( quat,'selectQuat','normal'); n1 = norm(theta1,2);
+            theta2 = mystica.rbm.logQuat(-quat,'selectQuat','normal'); n2 = norm(theta2,2);
+            theta = theta1*(n1<=n2)+theta2*(n2<n1);
+    end
+end
diff --git a/+mystica/+rbm/multiplyQuat.m b/+mystica/+rbm/multiplyQuat.m
new file mode 100644
index 0000000..148372b
--- /dev/null
+++ b/+mystica/+rbm/multiplyQuat.m
@@ -0,0 +1,10 @@
+function quat = multiplyQuat(quat1,quat2)
+
+    qw1 = quat1(1); qxyz1 = quat1(2:4);
+    qw2 = quat2(1); qxyz2 = quat2(2:4);
+
+    pw   = qw1*qw2 - qxyz1'*qxyz2;
+    pxyz = qw1*qxyz2 + qw2*qxyz1 + cross(qxyz1,qxyz2);
+
+    quat = [pw;pxyz];
+end
diff --git a/+mystica/+state/@StateKinMBody/getJacobianConstraints.m b/+mystica/+state/@StateKinMBody/getJacobianConstraints.m
index 87eb1c3..0fe9f8f 100644
--- a/+mystica/+state/@StateKinMBody/getJacobianConstraints.m
+++ b/+mystica/+state/@StateKinMBody/getJacobianConstraints.m
@@ -5,88 +5,105 @@ function getJacobianConstraints(obj,model)
         obj   mystica.state.StateKinMBody
         model mystica.model.Model
     end
-    
+
     Jc     = {};
     intJcV = {};
     cnstr_lin = {};
     cnstr_ang = {};
-    
+
     mBodyPosQuat_0_initial = casadi.SX.sym('x_initial',model.constants.mBodyPosQuat,1);
-    
+
     for i = transpose(model.indexesFixedLink(:))
-        
+
         sel_twist_i = model.linksAttributes{i}.selector.matrix_linkTwist_from_mBodyTwist;
-        linkPosQuat_0_initial = mBodyPosQuat_0_initial(model.linksAttributes{i}.selector.indexes_linkPosQuat_from_mBodyPosQuat);
-        
+        linkPosQuat_0_initial = mBodyPosQuat_0_initial(  model.linksAttributes{i}.selector.indexes_linkPosQuat_from_mBodyPosQuat);
+        linkPosQuat_0         = obj.csdSy.mBodyPosQuat_0(model.linksAttributes{i}.selector.indexes_linkPosQuat_from_mBodyPosQuat);
+
+        quat_bi_0 = mystica.rbm.invQuat(mystica.rbm.getQuatGivenPosQuat(linkPosQuat_0_initial));
+        quat_0_b  = mystica.rbm.getQuatGivenPosQuat(linkPosQuat_0);
+        pos_0_bi  = mystica.rbm.getPosGivenPosQuat( linkPosQuat_0_initial);
+        pos_0_b   = mystica.rbm.getPosGivenPosQuat( linkPosQuat_0);
+
         M = sel_twist_i;
-        N = [linkPosQuat_0_initial(1:model.constants.linkPos);sparse(model.constants.linkAngVel,1)];
-        
+        N = [pos_0_b-pos_0_bi;mystica.rbm.logQuat(mystica.rbm.multiplyQuat(quat_0_b,quat_bi_0),'selectQuat','minDistance')];
+
         Jc{       end+1} = M;
         intJcV{   end+1} = N;
         cnstr_lin{end+1} = [1 1 1 0 0 0]';
         cnstr_ang{end+1} = [0 0 0 1 1 1]';
     end
-    
+
     for i = 1 : model.nJoint
         index = model.joints{i}.getJointConnectionDetails();
-        
+
         tform_0_p  = obj.get_link_tform_b('iLink',index.parent,'model',model,'mBodyPosQuat_0',obj.csdSy.mBodyPosQuat_0);
         tform_0_c  = obj.get_link_tform_b('iLink',index.child ,'model',model,'mBodyPosQuat_0',obj.csdSy.mBodyPosQuat_0);
         tform_p_pj = model.linksAttributes{index.parent}.tform_b_j{index.cPointParent};
         tform_c_cj = model.linksAttributes{index.child }.tform_b_j{index.cPointChild };
-        
+        tform_0_pj = tform_0_p*tform_p_pj;
+        tform_0_cj = tform_0_c*tform_c_cj;
+
+        quat_0_p = mystica.rbm.getQuatGivenPosQuat(obj.csdSy.mBodyPosQuat_0(model.linksAttributes{index.parent}.selector.indexes_linkPosQuat_from_mBodyPosQuat));
+        quat_0_c = mystica.rbm.getQuatGivenPosQuat(obj.csdSy.mBodyPosQuat_0(model.linksAttributes{index.child }.selector.indexes_linkPosQuat_from_mBodyPosQuat));
+
+        quat_pj_0    = mystica.rbm.invQuat(mystica.rbm.multiplyQuat(quat_0_p,mystica.rbm.getQuatGivenTform(tform_p_pj)));
+        quat_0_cj    = mystica.rbm.multiplyQuat(quat_0_c,mystica.rbm.getQuatGivenTform(tform_c_cj));
+        quat_pj_cj   = mystica.rbm.multiplyQuat(quat_pj_0,quat_0_cj);
+
+        quat_cj0_pj0 = mystica.rbm.invQuat(mystica.rbm.getQuatGivenTform(model.joints{i}.tform_pj0_cj0));
+
         if strcmp(...
                 model.joints{i}.type,'spherical') || ...
                 ...strcmp(model.joints{i}.type,'universal') || ...  %toBeDone! see https://github.com/ami-iit/element_morphing-cover-design/issues/86
                 strcmp(model.joints{i}.type,'revolute')  || ...
                 strcmp(model.joints{i}.type,'fixed')
-            
+
             rotm_0_p    = mystica.rbm.getRotmGivenTform(tform_0_p);
             rotm_0_c    = mystica.rbm.getRotmGivenTform(tform_0_c);
             pos_p_pj    = mystica.rbm.getPosGivenTform(tform_p_pj);
             pos_c_cj    = mystica.rbm.getPosGivenTform(tform_c_cj);
             sel_twist_p = model.linksAttributes{index.parent}.selector.matrix_linkTwist_from_mBodyTwist;
             sel_twist_c = model.linksAttributes{index.child }.selector.matrix_linkTwist_from_mBodyTwist;
-            
+
             M = transpose(rotm_0_p)*[eye(3) -mystica.utils.skew( rotm_0_p * pos_p_pj )]*sel_twist_p - ...
                 transpose(rotm_0_p)*[eye(3) -mystica.utils.skew( rotm_0_c * pos_c_cj )]*sel_twist_c;
-            
-            N = transpose(rotm_0_p)*(mystica.rbm.getPosGivenTform(tform_0_p*tform_p_pj)-mystica.rbm.getPosGivenTform(tform_0_c*tform_c_cj));
-            
+
+            N = transpose(rotm_0_p)*(mystica.rbm.getPosGivenTform(tform_0_pj)-mystica.rbm.getPosGivenTform(tform_0_cj));
+
             Jc{       end+1} = M;
             intJcV{   end+1} = N;
             cnstr_lin{end+1} = ones( size(M,1),1);
             cnstr_ang{end+1} = zeros(size(M,1),1);
-            
+
         end
         if ...strcmp(model.joints{i}.type,'universal') || ...  %toBeDone! see https://github.com/ami-iit/element_morphing-cover-design/issues/86
                 strcmp(model.joints{i}.type,'revolute')  || ...
                 strcmp(model.joints{i}.type,'fixed')
-            
+
             constrainedDirections = diag(not(model.joints{i}.axesRotation));
             constrainedDirections = constrainedDirections(sum(constrainedDirections,2)==1,:)*1;
-            
+
             rotm_pj_0 = transpose(mystica.rbm.getRotmGivenTform(tform_0_p*tform_p_pj));
             sel_angVel_c = model.linksAttributes{index.child }.selector.matrix_linkAngVel_from_mBodyTwist;
             sel_angVel_p = model.linksAttributes{index.parent}.selector.matrix_linkAngVel_from_mBodyTwist;
-            
+
             M = constrainedDirections*rotm_pj_0*(sel_angVel_c - sel_angVel_p);
-            N = sparse(size(constrainedDirections,1),1);
-            
+            N = constrainedDirections*mystica.rbm.logQuat(mystica.rbm.multiplyQuat(quat_pj_cj,quat_cj0_pj0),'selectQuat','minDistance');
+
             Jc{       end+1} = M;
             intJcV{   end+1} = N;
             cnstr_lin{end+1} = zeros(size(M,1),1);
             cnstr_ang{end+1} = ones( size(M,1),1);
-            
+
         end
     end
-    
+
     obj.csdSy.Jc = vertcat(Jc{:});
     obj.csdFn.Jc = casadi.Function('Jc',{obj.csdSy.mBodyPosQuat_0},{obj.csdSy.Jc});
-    
+
     obj.csdSy.intJcV = vertcat(intJcV{:});
     obj.csdFn.intJcV = casadi.Function('intJcV',{obj.csdSy.mBodyPosQuat_0,mBodyPosQuat_0_initial},{obj.csdSy.intJcV});
-    
+
     model.appendSelectorConstrainedDirections('indexes_ang',find(vertcat(cnstr_ang{:})),'indexes_lin',find(vertcat(cnstr_lin{:})))
-    
+
 end
diff --git a/+mystica/+utils/skewVee.m b/+mystica/+utils/skewVee.m
index 7fbebc0..2651f1f 100644
--- a/+mystica/+utils/skewVee.m
+++ b/+mystica/+utils/skewVee.m
@@ -1,19 +1,16 @@
 function x = skewVee(X)
-% function x = skewVee(X)
+    % function x = skewVee(X)
 
     X_skew = (X - X.')/2;
 
-% X = [ 0       X(1,2)  X(1,3);
-%      -X(1,2)  0       X(2,3);
-%      -X(1,3)  X(2,3)  0  ];
+    % X = [ 0       X(1,2)  X(1,3);
+    %      -X(1,2)  0       X(2,3);
+    %      -X(1,3)  X(2,3)  0  ];
 
-% X = [ 0      -x(3)    x(2);
-%       x(3)    0      -x(1);
-%      -x(2)    x(1)    0  ];
+    % X = [ 0      -x(3)    x(2);
+    %       x(3)    0      -x(1);
+    %      -x(2)    x(1)    0  ];
 
-
-    x = [-X_skew(2,3);...
-          X_skew(1,3);...
-         -X_skew(1,2)];
+    x = [-X_skew(2,3);X_skew(1,3);-X_skew(1,2)];
 
 end