get smtgh from dev closed loop to master

Functions/AlgoMathsModel/Rodrigues.m

@@ -22,7 +22,6 @@
 	ap=[0 -a(3) a(2);...
          a(3) 0 -a(1);...
         -a(2) a(1) 0];
-	R=[1 0 0;0 1 0;0 0 1]+ap*sin(q)+ap*ap*(1-cos(q));
+	R=[1 0 0;0 1 0;0 0 1]+ap.*sin(q)+pagemtimes(ap,ap).*(1-cos(q));
 end
-
 end

Functions/Animation/DataExtractionForAnimation.m

@@ -97,10 +97,10 @@
                     q=q_complet(1:end-6);
                  end
          else
-                     if isfield(BiomechanicalModel,'ClosedLoopData')
-                        [~,q] = ForwardKinematicsConstrained(BiomechanicalModel,q);
-                        q = q(1:end-6);
-                     end
+%                      if isfield(BiomechanicalModel,'ClosedLoopData')
+%                         [~,q] = ForwardKinematicsConstrained(BiomechanicalModel,q);
+%                         q = q(1:end-6);
+%                      end
         end
 
 

Functions/Calibration/CalibrateModelGenerationNum.m

@@ -61,8 +61,8 @@
 %% Symbolic functions
 disp('Preliminary Computations ...')
   [BiomechanicalModel.OsteoArticularModel] = Add6dof(BiomechanicalModel.OsteoArticularModel);
-[BiomechanicalModel.OsteoArticularModel, ...
-    BiomechanicalModel.Generalized_Coordinates] = SymbolicFunctionGenerationIK(BiomechanicalModel.OsteoArticularModel,BiomechanicalModel.Markers);
+[BiomechanicalModel.OsteoArticularModel, BiomechanicalModel.Jacob, ...
+    BiomechanicalModel.Generalized_Coordinates] = SymbolicFunctionGenerationIK(BiomechanicalModel.OsteoArticularModel,BiomechanicalModel.Markers,BiomechanicalModel.GeometricalCalibration.radius);
 disp('... Preliminary Computations done')
 
 %% Inertial calibration
@@ -121,7 +121,7 @@
 
 %% Closedloop equations
 disp('Closed loop equations ...')
-BiomechanicalModel = AddClosedLoopEquations(BiomechanicalModel);
+BiomechanicalModel = AddClosedLoopEquations(BiomechanicalModel,BiomechanicalModel.GeometricalCalibration.radius);
 disp('... Closed loop equations  done')
 
 save('BiomechanicalModel','BiomechanicalModel');

Functions/Calibration/Geometrical/CostFunctionLMCalib.m

@@ -26,9 +26,8 @@
         fonc = hclosedloophandle{k};
         constraints = [constraints ; fonc(q)];
     end
-
-    dX = weights.*ik_function_objective(q);
-
-    func = [ dX ; gamma*constraints ; zeta*hbutees(q)];
+    newweights= repmat(weights,3,1);
+    dX = newweights(:).*ik_function_objective(q);
+    func = [ dX ; gamma*constraints(:) ; zeta*hbutees(q)];
 
 end

Functions/Calibration/Geometrical/CostFunctionSymbolicCalib.m

@@ -36,6 +36,8 @@
         (Pelvis_position,Pelvis_rotation,q,k,...
         pcut,Rcut) - real_markers(m).position(f,:)') ;
 end
+
+
 W=diag(weights);
 error=e'*W*e;
 

Functions/Calibration/Geometrical/ErrorMarkersCalibLM.m

@@ -0,0 +1,41 @@
+function [error] = ErrorMarkersCalibLM(q,k,real_markers,f,list_markers,Pelvis_position,Pelvis_rotation,Rcut,pcut,nbcut,list_function)
+
+% Computation of reconstruction error for the geometrical calibration
+%   Computation of the distance between the position of each experimental
+%   marker and the position of the corresponded model marker
+%
+%   INPUT
+%   - q: vector of joint coordinates at a given instant
+%   - k: vector of homothety coefficient
+%   - Pelvis_position: position of the pelvis at the considered instant
+%   - Pelvis_rotation: rotation of the pelvis at the considered instant
+%   - positions : matrix of experimental marker positions
+%   OUTPUT
+%   - error: distance between the position of each experimental marker and
+%   the position of the corresponded model marker
+%________________________________________________________
+%
+% Licence
+% Toolbox distributed under GPL 3.0 Licence
+%________________________________________________________
+%
+% Authors : Antoine Muller, Charles Pontonnier, Pierre Puchaud and
+% Georges Dumont
+%________________________________________________________
+
+
+
+for c=1:nbcut
+    if c==1
+        [Rcut(:,:,c),pcut(:,:,c)]=list_function{c}(Pelvis_position,Pelvis_rotation,q,k,[],[]);
+    else
+        [Rcut(:,:,c),pcut(:,:,c)]=list_function{c}(Pelvis_position,Pelvis_rotation,q,k,pcut,Rcut);
+    end
+end
+error=zeros(3*numel(list_markers),1);
+for m=1:numel(list_markers)
+    error(1+3*(m-1):3+3*(m-1),:) =   ( list_markers{m}...
+        (Pelvis_position,Pelvis_rotation,q,k,...
+        pcut,Rcut)  - real_markers(m).position(f,:)');
+end
+end

Functions/Calibration/Geometrical/GeomCalibOptimization.m

@@ -8,8 +8,9 @@
 
 nonlcon=@(qvar)ClosedLoopCalib(Base_position{f},Base_rotation{f},qvar,k_init); % testé!
 
-options1 = optimoptions(@fmincon,'Algorithm','interior-point','Display','off','TolFun',1e-2,'MaxFunEvals',2e5,'MaxIter',2e5);
+options1 = optimoptions(@fmincon,'Algorithm','interior-point','Display','off','TolFun',1e-4,'MaxFunEvals',2e5,'MaxIter',2e5);
 options2 = optimoptions(@fmincon,'Algorithm','interior-point','Display','off','TolFun',1e-6,'MaxFunEvals',2e9,'MaxIter',2e9);
+h = waitbar(0,['First iteration of Inverse Kinematics']);
 
 [q_value{1}(:,f)] = fmincon(ik_function_objective,q0,[],[],Aeq_ik,beq_ik,l_inf,l_sup,nonlcon,options1);
 
@@ -20,19 +21,21 @@
 buteehandle = @(q)  Limits(q,l_inf,l_sup);
 gamma = 150;
 zeta = 20;
-
-q0 = q_value{1}(:,f);
-
-parfor f = 1:nb_frame_calib
-
-    ik_function_objective=@(qvar) ErrorMarkersCalib(qvar,k_init,real_markers_calib,f,list_function_markers,Base_position{f},Base_rotation{f},Rcut,pcut,nbcut,list_function);
+waitbar(f/nb_frame_calib)
+q_inter = zeros(Nb_qred,nb_frame_calib);
+ik_function_objective=@(qvar) ErrorMarkersCalibLM(qvar,k_init,real_markers_calib,f,list_function_markers,Base_position{f},Base_rotation{f},Rcut,pcut,nbcut,list_function);
+hclosedloophandle = {@(qvar)ClosedLoopCalib(Base_position{f},Base_rotation{f},qvar,k_init);  @(x) Aeq_ik*x - beq_ik} ;
+fun = @(q) CostFunctionLMCalib(q,ik_function_objective,gamma,hclosedloophandle,zeta,buteehandle,weights);
+[q_inter(:,f)] = lsqnonlin(fun,q_inter(:,f),[],[],optionsLM);
+for f = 2:nb_frame_calib
+    ik_function_objective=@(qvar) ErrorMarkersCalibLM(qvar,k_init,real_markers_calib,f,list_function_markers,Base_position{f},Base_rotation{f},Rcut,pcut,nbcut,list_function);
     hclosedloophandle = {@(qvar)ClosedLoopCalib(Base_position{f},Base_rotation{f},qvar,k_init);  @(x) Aeq_ik*x - beq_ik} ;
-
     fun = @(q) CostFunctionLMCalib(q,ik_function_objective,gamma,hclosedloophandle,zeta,buteehandle,weights);
-
-    [q_inter(:,f)] = lsqnonlin(fun,q0,[],[],optionsLM);
+    [q_inter(:,f)] = lsqnonlin(fun,q_inter(:,f-1),[],[],optionsLM);
+    waitbar(f/nb_frame_calib)
 end
-q_value{1}(:,2:nb_frame_calib) = q_inter(:,2:nb_frame_calib);
+q_value{1}= q_inter;
+close(h)
 
 
 % Error computation
@@ -63,30 +66,25 @@
     [kp_opt(:,g+1)] = fmincon(pk_function_objective,kp_opt(:,g),[],[],Aeq_calib,beq_calib,limit_inf_calib,limit_sup_calib,[],options2);
 
     q_value{g+1}=zeros(size(q_value{g})); %#ok<AGROW>
-
+    q_inter=q_value{g};
     % Articular coordinates optimisation
 
-    q0=q_value{g}(:,f);
     kp_g = kp_opt(:,g+1);
     parfor f =1:nb_frame_calib
-
-        ik_function_objective=@(qvar) ErrorMarkersCalib(qvar,kp_g,real_markers_calib,f,list_function_markers,Base_position{f},Base_rotation{f},Rcut,pcut,nbcut,list_function);
+        ik_function_objective=@(qvar) ErrorMarkersCalibLM(qvar,kp_g,real_markers_calib,f,list_function_markers,Base_position{f},Base_rotation{f},Rcut,pcut,nbcut,list_function);
 
         hclosedloophandle = {@(qvar)ClosedLoopCalib(Base_position{f},Base_rotation{f},qvar,kp_g);  @(x) Aeq_ik*x - beq_ik} ;
 
         fun = @(q) CostFunctionLMCalib(q,ik_function_objective,gamma,hclosedloophandle,zeta,buteehandle,weights);
-                [q_inter(:,f)] = lsqnonlin(fun,q0,[],[],optionsLM);
-
-
+        [q_inter(:,f)] = lsqnonlin(fun,q_inter(:,f),[],[],optionsLM);
     end
     q_value{g+1} = q_inter;
 
     % Error computation
     errorm{g+1}=zeros(length(real_markers_calib),nb_frame_calib); %#ok<AGROW>
     for f=1:nb_frame_calib
-
-        [errorm{g+1}(:,f)] = ErrorMarkersCalib(q_value{g+1}(:,f),kp_opt(:,g+1),real_markers_calib,f,list_function_markers,Base_position{f},Base_rotation{f},Rcut,pcut,nbcut,list_function);
-
+        temp = weights.*ErrorMarkersCalib(q_value{g+1}(:,f),kp_opt(:,g+1),real_markers_calib,f,list_function_markers,Base_position{f},Base_rotation{f},Rcut,pcut,nbcut,list_function);
+        [errorm{g+1}(:,f)] = temp(weights~=0);
     end
 
     % Stop criteria