Finalize the initialization to compensate gravity

src/initializeNanoHexapod.m

@@ -33,6 +33,8 @@ arguments
     args.ARB (3,3) double {mustBeNumeric} = eye(3)
     % Equilibrium position of each leg
     args.dLeq (6,1) double {mustBeNumeric} = zeros(6,1)
+    % Force that stiffness of each joint should apply at t=0
+    args.Foffset      logical {mustBeNumericOrLogical} = false
 end
 
 nano_hexapod = initializeFramesPositions('H', args.H, 'MO_B', args.MO_B);
@@ -52,7 +54,12 @@ nano_hexapod = computeJacobian(nano_hexapod);
 nano_hexapod.Li = Li;
 nano_hexapod.dLi = dLi;
 
-nano_hexapod.dLeq = args.dLeq;
+if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
+  load('mat/Foffset.mat', 'Fnm');
+  nano_hexapod.dLeq = -Fnm'./nano_hexapod.Ki;
+else
+  nano_hexapod.dLeq = args.dLeq;
+end
 
 switch args.type
   case 'none'