Verified the function that converts the error in the NASS base

src/inverseKinematicsHexapod.m

@@ -0,0 +1,31 @@
+% Inverse Kinematics of the Hexapod
+% :PROPERTIES:
+% :header-args:matlab+: :tangle ../src/inverseKinematicsHexapod.m
+% :header-args:matlab+: :comments org :mkdirp yes
+% :header-args:matlab+: :eval no :results none
+% :END:
+% <<sec:inverseKinematicsHexapod>>
+
+% This Matlab function is accessible [[file:src/inverseKinematicsHexapod.m][here]].
+
+
+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