Huge Update

- Modify the joints for Ty, Ry, Rz to have only one bushing joint.
- Compensation of gravity

functions/index.org

@@ -284,38 +284,6 @@ This Matlab function is accessible [[file:../src/converErrorBasis.m][here]].
   end
 #+end_src
 
-* Inverse Kinematics of the Hexapod
-:PROPERTIES:
-:header-args:matlab+: :tangle ../src/inverseKinematicsHexapod.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:inverseKinematicsHexapod>>
-
-This Matlab function is accessible [[file:src/inverseKinematicsHexapod.m][here]].
-
-#+begin_src matlab
-  function [L] = inverseKinematicsHexapod(hexapod, AP, ARB)
-  % inverseKinematicsHexapod - Compute the initial position of each leg to have the wanted Hexapod's position
-  %
-  % Syntax: inverseKinematicsHexapod(hexapod, AP, ARB)
-  %
-  % Inputs:
-  %    - hexapod - Hexapod object containing the geometry of the hexapod
-  %    - AP - Position vector of point OB expressed in frame {A} in [m]
-  %    - ARB - Rotation Matrix expressed in frame {A}
-
-    % Wanted Length of the hexapod's legs [m]
-    L = zeros(6, 1);
-
-    for i = 1:length(L)
-      Bbi = hexapod.pos_top_tranform(i, :)' - 1e-3*[0 ; 0 ; hexapod.TP.thickness+hexapod.Leg.sphere.top+hexapod.SP.thickness.top+hexapod.jacobian]; % [m]
-      Aai = hexapod.pos_base(i, :)' + 1e-3*[0 ; 0 ; hexapod.BP.thickness+hexapod.Leg.sphere.bottom+hexapod.SP.thickness.bottom-hexapod.h-hexapod.jacobian]; % [m]
-
-      L(i) = sqrt(AP'*AP + Bbi'*Bbi + Aai'*Aai - 2*AP'*Aai + 2*AP'*(ARB*Bbi) - 2*(ARB*Bbi)'*Aai);
-    end
-  end
-#+end_src
-
 * computeReferencePose
 :PROPERTIES:
 :header-args:matlab+: :tangle ../src/computeReferencePose.m
@@ -398,7 +366,7 @@ This Matlab function is accessible [[file:src/computeReferencePose.m][here]].
           0 0 1 Dn(3) ;
           0 0 0 1 ];
 
-    Rn(1:3, 1:3) = Rnx*Rny*Rnz;
+    Rn(1:3, 1:3) = Rnz*Rny*Rnx;
 
     %% Total Homogeneous transformation
     WTr = Rty*Rry*Rrz*Rh*Rn;