diff --git a/Examples/Cycling_Processed/JOTH_Fin_125HzModif/InverseKinematicsResults.mat b/Examples/Cycling_Processed/JOTH_Fin_125HzModif/InverseKinematicsResults.mat index 889fb3a7..5d16b483 100644 Binary files a/Examples/Cycling_Processed/JOTH_Fin_125HzModif/InverseKinematicsResults.mat and b/Examples/Cycling_Processed/JOTH_Fin_125HzModif/InverseKinematicsResults.mat differ diff --git a/Functions/AlgoMathsModel/Rodrigues.m b/Functions/AlgoMathsModel/Rodrigues.m index 50266b97..28cb8777 100644 --- a/Functions/AlgoMathsModel/Rodrigues.m +++ b/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 \ No newline at end of file diff --git a/Functions/Animation/DataExtractionForAnimation.m b/Functions/Animation/DataExtractionForAnimation.m index 08251167..e7bf6d9d 100644 --- a/Functions/Animation/DataExtractionForAnimation.m +++ b/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 diff --git a/Functions/Calibration/CalibrateModelGenerationNum.m b/Functions/Calibration/CalibrateModelGenerationNum.m index 3ccea8fc..15d03565 100644 --- a/Functions/Calibration/CalibrateModelGenerationNum.m +++ b/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'); diff --git a/Functions/Calibration/Geometrical/CostFunctionLMCalib.m b/Functions/Calibration/Geometrical/CostFunctionLMCalib.m index f7ded408..25995fe2 100644 --- a/Functions/Calibration/Geometrical/CostFunctionLMCalib.m +++ b/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 \ No newline at end of file diff --git a/Functions/Calibration/Geometrical/CostFunctionSymbolicCalib.m b/Functions/Calibration/Geometrical/CostFunctionSymbolicCalib.m index e611994b..56579644 100644 --- a/Functions/Calibration/Geometrical/CostFunctionSymbolicCalib.m +++ b/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; diff --git a/Functions/Calibration/Geometrical/ErrorMarkersCalibLM.m b/Functions/Calibration/Geometrical/ErrorMarkersCalibLM.m new file mode 100644 index 00000000..69485466 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/Functions/Calibration/Geometrical/GeomCalibOptimization.m b/Functions/Calibration/Geometrical/GeomCalibOptimization.m index 1213c40d..fea44102 100644 --- a/Functions/Calibration/Geometrical/GeomCalibOptimization.m +++ b/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 - + 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 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 diff --git a/Functions/Calibration/Geometrical/GeometricalCalibration.m b/Functions/Calibration/Geometrical/GeometricalCalibration.m index 09dda680..d3ca2474 100644 --- a/Functions/Calibration/Geometrical/GeometricalCalibration.m +++ b/Functions/Calibration/Geometrical/GeometricalCalibration.m @@ -233,7 +233,8 @@ calib_parameters.kp_opt_scap = kp_opt_scap(:,end); k_init = kp_opt_scap(:,end); end -weights=ones(length(list_markers),1); +% weights=ones(length(list_markers),1); +weights =(ones(1,length(real_markers)).*[real_markers(:).weight])'; [kp_opt,crit,errorm,q0]=GeomCalibOptimization(k_init,weights,Nb_qred,nb_frame_calib,Base_position,Base_rotation,list_function,Rcut,pcut,real_markers_calib,nbcut,list_function_markers,Aeq_ik,beq_ik,l_inf,l_sup,Aeq_calib,beq_calib); calib_parameters.crit = crit; calib_parameters.errorm = errorm; @@ -259,7 +260,7 @@ for i=1:numel(Human_model_save) Human_model_calib(i).b=Human_model_save(i).b*calib_parameters.k_calib(OsteoArticularModel(i).mother); % b (k de la m�re) Human_model_calib(i).c=Human_model_save(i).c*calib_parameters.k_calib(i); % c - Human_model_calib(i).I=Human_model_save(i).I*calib_parameters.k_calib(i); % I + Human_model_calib(i).I=Human_model_save(i).I*calib_parameters.k_calib(i)*calib_parameters.k_calib(i); % I for j=1:size(Human_model_save(i).anat_position,1) Human_model_calib(i).anat_position{j,2}=Human_model_save(i).anat_position{j,2}*calib_parameters.k_calib(i); end @@ -299,85 +300,193 @@ switch Human_model_calib(j).name case 'RScapuloThoracic_J1' - - syms theta phi real % latitude longitude - - Human_model_calib(j).kinematic_dependancy.q=matlabFunction(-calib_parameters.radius(1)*cos(theta)*cos(phi),'vars',{theta,phi}); - + if sum(contains({Human_model_calib.name},'RScapuloThoracic_Jalpha')) + syms theta1 theta2 real + t1 = -calib_parameters.radius(1)*cos(theta1)*cos(theta2); + ft =matlabFunction(t1,'vars',[theta1,theta2]); + dft =matlabFunction(jacobian(t1,[theta1,theta2]),'vars',[theta1,theta2]); + ddft =matlabFunction(jacobian(jacobian(t1,[theta1,theta2]),[theta1,theta2]),'vars',[theta1,theta2]); + Human_model_calib(j).kinematic_dependancy.q= ft; + Human_model_calib(j).kinematic_dependancy.dq=dft; + Human_model_calib(j).kinematic_dependancy.ddq=ddft; + else + syms theta phi real % latitude longitude + + t1 = -calib_parameters.radius(1)*cos(theta)*cos(phi); + ft =matlabFunction(t1,'vars',[theta,phi]); + dft =matlabFunction(jacobian(t1,[theta,phi]),'vars',[theta,phi]); + ddft =matlabFunction(jacobian(jacobian(t1,[theta,phi]),[theta,phi]),'vars',[theta,phi]); + Human_model_calib(j).kinematic_dependancy.q= ft; + Human_model_calib(j).kinematic_dependancy.dq=dft; + Human_model_calib(j).kinematic_dependancy.ddq=ddft; + end case 'RScapuloThoracic_J2' - - syms theta real% latitude - - Human_model_calib(j).kinematic_dependancy.q=matlabFunction(calib_parameters.radius(2)*sin(theta),'vars',{theta}); - + if sum(contains({Human_model_calib.name},'RScapuloThoracic_Jalpha')) + syms theta2 real + t1 = calib_parameters.radius(2)*sin(theta2); + ft =matlabFunction(t1,'vars',[theta2]); + dft =matlabFunction(jacobian(t1,[theta2]),'vars',[theta2]); + ddft =matlabFunction(jacobian(jacobian(t1,[theta2]),[theta2]),'vars',[theta2]); + Human_model_calib(j).kinematic_dependancy.q=ft; + Human_model_calib(j).kinematic_dependancy.dq=dft; + Human_model_calib(j).kinematic_dependancy.ddq=ddft; + else + syms theta real% latitude + + t1 = calib_parameters.radius(2)*sin(theta); + + ft =matlabFunction(t1,'vars',[theta]); + dft =matlabFunction(jacobian(t1,[theta]),'vars',[theta]); + ddft =matlabFunction(jacobian(jacobian(t1,[theta]),[theta]),'vars',[theta]); + Human_model_calib(j).kinematic_dependancy.q= ft; + Human_model_calib(j).kinematic_dependancy.dq=dft; + Human_model_calib(j).kinematic_dependancy.ddq=ddft; + end case 'RScapuloThoracic_J3' - syms theta phi real % latitude longitude - - Human_model_calib(j).kinematic_dependancy.q=matlabFunction(calib_parameters.radius(3)*cos(theta)*sin(phi),'vars',{theta,phi}); + if sum(contains({Human_model_calib.name},'RScapuloThoracic_Jalpha')) + syms theta1 theta2 real + t1 = calib_parameters.radius(3)*sin(theta1)*cos(theta2); + ft =matlabFunction(t1,'vars',[theta1,theta2]); + dft =matlabFunction(jacobian(t1,[theta1,theta2]),'vars',[theta1,theta2]); + ddft =matlabFunction(jacobian(jacobian(t1,[theta1,theta2]),[theta1,theta2]),'vars',[theta1,theta2]); + Human_model_calib(j).kinematic_dependancy.q= ft; + Human_model_calib(j).kinematic_dependancy.dq=dft; + Human_model_calib(j).kinematic_dependancy.ddq=ddft; + else + syms theta phi real % latitude longitude + + t1 = calib_parameters.radius(3)*cos(theta)*sin(phi); + + ft =matlabFunction(t1,'vars',[theta,phi]); + dft =matlabFunction(jacobian(t1,[theta,phi]),'vars',[theta,phi]); + ddft =matlabFunction(jacobian(jacobian(t1,[theta,phi]),[theta,phi]),'vars',[theta,phi]); + Human_model_calib(j).kinematic_dependancy.q= ft; + Human_model_calib(j).kinematic_dependancy.dq=dft; + Human_model_calib(j).kinematic_dependancy.ddq=ddft; + end case 'LScapuloThoracic_J1' - syms theta phi real % latitude longitude - - Human_model_calib(j).kinematic_dependancy.q=matlabFunction(-calib_parameters.radius(4)*cos(theta)*cos(phi),'vars',{theta,phi}); - - + if sum(contains({Human_model_calib.name},'LScapuloThoracic_Jalpha')) + syms theta1 theta2 real + t1 = -calib_parameters.radius(4)*cos(theta1)*cos(theta2); + ft =matlabFunction(t1,'vars',[theta1,theta2]); + dft =matlabFunction(jacobian(t1,[theta1,theta2]),'vars',[theta1,theta2]); + ddft =matlabFunction(jacobian(jacobian(t1,[theta1,theta2]),[theta1,theta2]),'vars',[theta1,theta2]); + Human_model_calib(j).kinematic_dependancy.q= ft; + Human_model_calib(j).kinematic_dependancy.dq=dft; + Human_model_calib(j).kinematic_dependancy.ddq=ddft; + else + syms theta phi real % latitude longitude + + t1=-calib_parameters.radius(4)*cos(theta)*cos(phi); + + ft =matlabFunction(t1,'vars',[theta,phi]); + dft =matlabFunction(jacobian(t1,[theta,phi]),'vars',[theta,phi]); + ddft =matlabFunction(jacobian(jacobian(t1,[theta,phi]),[theta,phi]),'vars',[theta,phi]); + Human_model_calib(j).kinematic_dependancy.q= ft; + Human_model_calib(j).kinematic_dependancy.dq=dft; + Human_model_calib(j).kinematic_dependancy.ddq=ddft; + + end case 'LScapuloThoracic_J2' - - syms theta real% latitude - - Human_model_calib(j).kinematic_dependancy.q=matlabFunction(calib_parameters.radius(5)*sin(theta),'vars',{theta}); - + if sum(contains({Human_model_calib.name},'LScapuloThoracic_Jalpha')) + syms theta2 real + t1 = calib_parameters.radius(5)*sin(theta2); + ft =matlabFunction(t1,'vars',[theta2]); + dft =matlabFunction(jacobian(t1,[theta2]),'vars',[theta2]); + ddft =matlabFunction(jacobian(jacobian(t1,[theta2]),[theta2]),'vars',[theta2]); + Human_model_calib(j).kinematic_dependancy.q=ft; + Human_model_calib(j).kinematic_dependancy.dq=dft; + Human_model_calib(j).kinematic_dependancy.ddq=ddft; + + else + syms theta real% latitude + + t1 = calib_parameters.radius(5)*sin(theta); + ft =matlabFunction(t1,'vars',[theta]); + dft =matlabFunction(jacobian(t1,[theta]),'vars',[theta]); + ddft =matlabFunction(jacobian(jacobian(t1,[theta]),[theta]),'vars',[theta]); + Human_model_calib(j).kinematic_dependancy.q= ft; + Human_model_calib(j).kinematic_dependancy.dq=dft; + Human_model_calib(j).kinematic_dependancy.ddq=ddft; + + end case 'LScapuloThoracic_J3' - - syms theta phi real % latitude longitude - - Human_model_calib(j).kinematic_dependancy.q=matlabFunction(-calib_parameters.radius(6)*cos(theta)*sin(phi),'vars',{theta,phi}); - - - case 'RScapuloThoracic_J5' - if sum(contains({Human_model_calib.name},'RScapuloThoracic_Jalpha')) + if sum(contains({Human_model_calib.name},'LScapuloThoracic_Jalpha')) + syms theta1 theta2 real + t1 = -calib_parameters.radius(6)*sin(theta1)*cos(theta2); + ft =matlabFunction(t1,'vars',[theta1,theta2]); + dft =matlabFunction(jacobian(t1,[theta1,theta2]),'vars',[theta1,theta2]); + ddft =matlabFunction(jacobian(jacobian(t1,[theta1,theta2]),[theta1,theta2]),'vars',[theta1,theta2]); + Human_model_calib(j).kinematic_dependancy.q= ft; + Human_model_calib(j).kinematic_dependancy.dq=dft; + Human_model_calib(j).kinematic_dependancy.ddq=ddft; + else + syms theta phi real % latitude longitude + + t1=-calib_parameters.radius(6)*cos(theta)*sin(phi); - syms phi lambda % latitude longitude + ft =matlabFunction(t1,'vars',[theta,phi]); + dft =matlabFunction(jacobian(t1,[theta,phi]),'vars',[theta,phi]); + ddft =matlabFunction(jacobian(jacobian(t1,[theta,phi]),[theta,phi]),'vars',[theta,phi]); + Human_model_calib(j).kinematic_dependancy.q= ft; + Human_model_calib(j).kinematic_dependancy.dq=dft; + Human_model_calib(j).kinematic_dependancy.ddq=ddft; - Human_model_calib(j).kinematic_dependancy.q=matlabFunction(atan((calib_parameters.radius(3)*calib_parameters.radius(2)*tan(lambda)*(1 - tan(phi)^2))/(calib_parameters.radius(1)*calib_parameters.radius(2)-calib_parameters.radius(1)*calib_parameters.radius(3)*tan(phi)^2))... - ,'vars',{phi,lambda}); + end + case 'RScapuloThoracic_J5' + if sum(contains({Human_model_calib.name},'RScapuloThoracic_Jalpha')) + syms theta1 theta2 real % latitude longitude + t1 = atan(-(calib_parameters.radius(1)*calib_parameters.radius(3)*tan(theta2)*(1 + tan(theta1)^2))/(calib_parameters.radius(3)*calib_parameters.radius(2)+calib_parameters.radius(1)*calib_parameters.radius(2)*tan(theta1)^2)); + ft =matlabFunction(t1,'vars',[theta1,theta2]); + dft =matlabFunction(jacobian(t1,[theta1,theta2]),'vars',[theta1,theta2]); + ddft =matlabFunction(jacobian(jacobian(t1,[theta1,theta2]),[theta1,theta2]),'vars',[theta1,theta2]); + Human_model_calib(j).kinematic_dependancy.q=ft; + Human_model_calib(j).kinematic_dependancy.dq=dft; + Human_model_calib(j).kinematic_dependancy.ddq=ddft; end case 'RScapuloThoracic_Jalpha' - syms phi + syms theta1 theta2 real + phi = atan(-(calib_parameters.radius(1)*calib_parameters.radius(3)*tan(theta2)*(1 + tan(theta1)^2))/(calib_parameters.radius(3)*calib_parameters.radius(2)+calib_parameters.radius(1)*calib_parameters.radius(2)*tan(theta1)^2)); + t1=atan(tan(theta1)*(-calib_parameters.radius(2)/calib_parameters.radius(3)*sin(phi)/tan(theta2)-cos(phi))); + ft =matlabFunction(t1,'vars',[theta1,theta2]); + dft =matlabFunction(jacobian(t1,[theta1,theta2]),'vars',[theta1,theta2]); + ddft =matlabFunction(jacobian(jacobian(t1,[theta1,theta2]),[theta1,theta2]),'vars',[theta1,theta2]); + Human_model_calib(j).kinematic_dependancy.q=ft; + Human_model_calib(j).kinematic_dependancy.dq=dft; + Human_model_calib(j).kinematic_dependancy.ddq=ddft; - Human_model_calib(j).kinematic_dependancy.q=matlabFunction(atan( tan(phi)*(calib_parameters.radius(3)*(1 - tan(phi)^2)/(calib_parameters.radius(2) - calib_parameters.radius(3)*tan(phi)^2) -1))... - ,'vars',{phi,lambda}); case 'LScapuloThoracic_J5' if sum(contains({Human_model_calib.name},'LScapuloThoracic_Jalpha')) - - syms phi lambda % latitude longitude - - Human_model_calib(j).kinematic_dependancy.q=matlabFunction(atan((calib_parameters.radius(6)*calib_parameters.radius(5)*tan(lambda)*(1 - tan(phi)^2))/(calib_parameters.radius(4)*calib_parameters.radius(5)-calib_parameters.radius(4)*calib_parameters.radius(6)*tan(phi)^2))... - ,'vars',{phi,lambda}); + syms theta1 theta2 real % latitude longitude + t1 = atan(-(calib_parameters.radius(4)*calib_parameters.radius(6)*tan(theta2)*(1 + tan(theta1)^2))/(calib_parameters.radius(6)*calib_parameters.radius(5)+calib_parameters.radius(4)*calib_parameters.radius(5)*tan(theta1)^2)); + ft =matlabFunction(t1,'vars',[theta1,theta2]); + dft =matlabFunction(jacobian(t1,[theta1,theta2]),'vars',[theta1,theta2]); + ddft =matlabFunction(jacobian(jacobian(t1,[theta1,theta2]),[theta1,theta2]),'vars',[theta1,theta2]); + Human_model_calib(j).kinematic_dependancy.q=ft; + Human_model_calib(j).kinematic_dependancy.dq=dft; + Human_model_calib(j).kinematic_dependancy.ddq=ddft; end - case 'LScapuloThoracic_Jalpha' - - syms phi - - - Human_model_calib(j).kinematic_dependancy.q=matlabFunction(atan( tan(phi)*(calib_parameters.radius(6)*(1 - tan(phi)^2)/(calib_parameters.radius(5) - calib_parameters.radius(6)*tan(phi)^2) -1))... - ,'vars',{phi,lambda}); - + case 'LScapuloThoracic_Jalpha' + syms theta1 theta2 real + phi= atan(-(calib_parameters.radius(4)*calib_parameters.radius(6)*tan(theta2)*(1 + tan(theta1)^2))/(calib_parameters.radius(6)*calib_parameters.radius(5)+calib_parameters.radius(4)*calib_parameters.radius(5)*tan(theta1)^2)); + t1=atan(tan(theta1)*(-calib_parameters.radius(5)/calib_parameters.radius(6)*sin(phi)/tan(theta2)-cos(phi))); + ft =matlabFunction(t1,'vars',[theta1,theta2]); + dft =matlabFunction(jacobian(t1,[theta1,theta2]),'vars',[theta1,theta2]); + ddft =matlabFunction(jacobian(jacobian(t1,[theta1,theta2]),[theta1,theta2]),'vars',[theta1,theta2]); + Human_model_calib(j).kinematic_dependancy.q=ft; + Human_model_calib(j).kinematic_dependancy.dq=dft; + Human_model_calib(j).kinematic_dependancy.ddq=ddft; end - - - - - - + end end diff --git a/Functions/Calibration/Geometrical/Symbolic_ForwardKinematicsCoupure_A.m b/Functions/Calibration/Geometrical/Symbolic_ForwardKinematicsCoupure_A.m index 32a419dc..3e7bf4eb 100644 --- a/Functions/Calibration/Geometrical/Symbolic_ForwardKinematicsCoupure_A.m +++ b/Functions/Calibration/Geometrical/Symbolic_ForwardKinematicsCoupure_A.m @@ -59,10 +59,9 @@ case 'RScapuloThoracic_J1' if sum(contains({Human_model.name},'RScapuloThoracic_Jalpha')) - - [~,idx] = intersect({Human_model.name},'RScapuloThoracic_J0'); - - q= -radius(1)*cos(theta)*cos(phi); + [~,idx_theta1] = intersect({Human_model.name},'RScapuloThoracic_J4'); + [~,idx_theta2] = intersect({Human_model.name},'RScapuloThoracic_J5bis'); + q= radius(1)*cos(Q(idx_theta1))*cos(Q(idx_theta2)); else %theta elevation [~,idx_theta] = intersect({Human_model.name},'RScapuloThoracic_J4'); @@ -76,9 +75,8 @@ case 'RScapuloThoracic_J2' if sum(contains({Human_model.name},'RScapuloThoracic_Jalpha')) - [~,idx1] = intersect({Human_model.name},'RScapuloThoracic_J4'); - [~,idx2] = intersect({Human_model.name},'RScapuloThoracic_J0'); - q= -radius(2)*sin(Q(idx1))*cos(Q(idx2)); + [~,idx_theta2] = intersect({Human_model.name},'RScapuloThoracic_J5bis'); + q= radius(2)*sin(Q(idx_theta2)); else @@ -86,16 +84,16 @@ [~,idx_theta] = intersect({Human_model.name},'RScapuloThoracic_J4'); q= radius(2)*sin(Q(idx_theta)); - + end case 'RScapuloThoracic_J3' if sum(contains({Human_model.name},'RScapuloThoracic_Jalpha')) - [~,idx1] = intersect({Human_model.name},'RScapuloThoracic_J4'); - [~,idx2] = intersect({Human_model.name},'RScapuloThoracic_J0'); - q= radius(3)*cos(Q(idx1))*cos(Q(idx2)); + [~,idx_theta1] = intersect({Human_model.name},'RScapuloThoracic_J4'); + [~,idx_theta2] = intersect({Human_model.name},'RScapuloThoracic_J5bis'); + q= radius(3)*sin(Q(idx_theta1))*cos(Q(idx_theta2)); else @@ -111,9 +109,9 @@ if sum(contains({Human_model.name},'LScapuloThoracic_Jalpha')) - - [~,idx] = intersect({Human_model.name},'LScapuloThoracic_J0'); - q= radius(4)*sin(Q(idx)); + [~,idx_theta1] = intersect({Human_model.name},'LScapuloThoracic_J4'); + [~,idx_theta2] = intersect({Human_model.name},'LScapuloThoracic_J5bis'); + q= radius(4)*cos(Q(idx_theta1))*cos(Q(idx_theta2)); else @@ -123,7 +121,7 @@ [~,idx_phi] = intersect({Human_model.name},'LScapuloThoracic_J5'); q= -radius(4)*cos(Q(idx_theta))*cos(Q(idx_phi)); - + end @@ -133,26 +131,25 @@ if sum(contains({Human_model.name},'LScapuloThoracic_Jalpha')) - [~,idx1] = intersect({Human_model.name},'LScapuloThoracic_J4'); - [~,idx2] = intersect({Human_model.name},'LScapuloThoracic_J0'); - q= -radius(5)*sin(Q(idx1))*cos(Q(idx2)); + [~,idx_theta2] = intersect({Human_model.name},'LScapuloThoracic_J5bis'); + q= radius(5)*sin(Q(idx_theta2)); else %theta elevation [~,idx_theta] = intersect({Human_model.name},'LScapuloThoracic_J4'); - + q=radius(5)*sin(Q(idx_theta)); - + end case 'LScapuloThoracic_J3' if sum(contains({Human_model.name},'LScapuloThoracic_Jalpha')) - [~,idx1] = intersect({Human_model.name},'LScapuloThoracic_J4'); - [~,idx2] = intersect({Human_model.name},'LScapuloThoracic_J0'); - q= -radius(6)*cos(Q(idx1))*cos(Q(idx2)); + [~,idx_theta1] = intersect({Human_model.name},'LScapuloThoracic_J4'); + [~,idx_theta2] = intersect({Human_model.name},'LScapuloThoracic_J5bis'); + q= -radius(6)*sin(Q(idx_theta1))*cos(Q(idx_theta2)); else @@ -162,7 +159,7 @@ [~,idx_phi] = intersect({Human_model.name},'LScapuloThoracic_J5'); q=-radius(6)*cos(Q(idx_theta))*sin(Q(idx_phi)); - + end @@ -170,33 +167,34 @@ case 'RScapuloThoracic_J5' if sum(contains({Human_model.name},'RScapuloThoracic_Jalpha')) - [~,idx1] = intersect({Human_model.name},'RScapuloThoracic_J4'); - [~,idx2] = intersect({Human_model.name},'RScapuloThoracic_J0'); - q = atan((radius(3)*radius(2)*tan(Q(idx2))*(1 - tan( Q(idx1))^2))/(radius(1)*radius(2)-radius(1)*radius(3)*tan( Q(idx1))^2)); + [~,idx_theta1] = intersect({Human_model.name},'RScapuloThoracic_J4'); + [~,idx_theta2] = intersect({Human_model.name},'RScapuloThoracic_J5bis'); + q = atan(-(radius(1)*radius(3)*tan(Q(idx_theta2))*(1 + tan(Q(idx_theta1))^2))/(radius(3)*radius(2)+radius(1)*radius(2)*tan(Q(idx_theta1))^2)); end case 'RScapuloThoracic_Jalpha' + [~,idx_theta1] = intersect({Human_model.name},'RScapuloThoracic_J4'); + [~,idx_theta2] = intersect({Human_model.name},'RScapuloThoracic_J5bis'); + phi= atan(-(radius(1)*radius(3)*tan(Q(idx_theta2))*(1 + tan(Q(idx_theta1))^2))/(radius(3)*radius(2)+radius(1)*radius(2)*tan(Q(idx_theta1))^2)); + q= atan(tan(Q(idx_theta1))*(-radius(2)/radius(3)*sin(phi)/tan(Q(idx_theta2))-cos(phi))); - [~,idx] = intersect({Human_model.name},'RScapuloThoracic_J4'); - - q= atan( tan(Q(idx))*(radius(3)*(1 - tan(Q(idx))^2)/(radius(2) - radius(3)*tan(Q(idx))^2) -1)); - case 'LScapuloThoracic_J5' if sum(contains({Human_model.name},'LScapuloThoracic_Jalpha')) - [~,idx1] = intersect({Human_model.name},'LScapuloThoracic_J4'); - [~,idx2] = intersect({Human_model.name},'LScapuloThoracic_J0'); - q = atan((radius(6)*radius(5)*tan(Q(idx2))*(1 - tan( Q(idx1))^2))/(radius(4)*radius(5)-radius(4)*radius(6)*tan( Q(idx1))^2)); + [~,idx_theta1] = intersect({Human_model.name},'LScapuloThoracic_J4'); + [~,idx_theta2] = intersect({Human_model.name},'LScapuloThoracic_J5bis'); + q = atan(-(radius(4)*radius(6)*tan(Q(idx_theta2))*(1 + tan(Q(idx_theta1))^2))/(radius(6)*radius(5)+radius(4)*radius(5)*tan(Q(idx_theta1))^2)); end case 'LScapuloThoracic_Jalpha' - [~,idx] = intersect({Human_model.name},'LScapuloThoracic_J4'); - - q= atan( tan(Q(idx))*(radius(3)*(1 - tan(Q(idx))^2)/(radius(2) - radius(3)*tan(Q(idx))^2) -1)); + [~,idx_theta1] = intersect({Human_model.name},'LScapuloThoracic_J4'); + [~,idx_theta2] = intersect({Human_model.name},'LScapuloThoracic_J5bis'); + phi=atan(-(radius(4)*radius(6)*tan(Q(idx_theta2))*(1 + tan(Q(idx_theta1))^2))/(radius(6)*radius(5)+radius(4)*radius(5)*tan(Q(idx_theta1))^2)); + q= atan(tan(Q(idx_theta1))*(-radius(5)/radius(6)*sin(phi)/tan(Q(idx_theta2))-cos(phi))); end @@ -229,6 +227,7 @@ eval(['assume(R' num2str(num_cut) 'cut(' num2str(zz) ',' num2str(z) '),''real'');']) end end + if Human_model(j).joint == 1 % liaison pivot (pin joint) % Human_model(j).p=eval(['R' num2str(num_cut) 'cut'])*(k(i)*Human_model(j).b)+eval(['p' num2str(num_cut) 'cut']); % position du rep�re (reference frame position) Human_model(j).p=eval(['R' num2str(num_cut) 'cut'])*(k(i)*Human_model(j).b)+eval(['p' num2str(num_cut) 'cut']); % position du rep�re (reference frame position) @@ -276,7 +275,7 @@ end end [solid_path1,solid_path2]=find_solid_path(Human_model,j,num_solid); - [c{numClosedLoop},ceq{numClosedLoop}]=NonLinCon_ClosedLoop_Sym(Human_model,solid_path1,solid_path2,num_solid,num_markers,Q,k); + [c{numClosedLoop},ceq{numClosedLoop}]=NonLinCon_ClosedLoop_Sym(Human_model,solid_path1,solid_path2,num_solid,num_markers,Q,k,radius); numClosedLoop=numClosedLoop+1; end diff --git a/Functions/Calibration/Scapula/AjouterScaploc.m b/Functions/Calibration/Scapula/AjouterScaploc.m new file mode 100644 index 00000000..d6df5a43 --- /dev/null +++ b/Functions/Calibration/Scapula/AjouterScaploc.m @@ -0,0 +1,17 @@ +function AjouterScaploc(filename) + + + real_markers=SplineScapLocTransformation(Lastframe,Firstframe,real_markers,O_SCAP_ar,O_SCAP_mi,O_SCAP_av,... +PSI_SCAPLOC_vers_epine_av, PSI_SCAPLOC_vers_epine_mi,PSI_SCAPLOC_vers_epine_ar,... +THETA_SCAPLOC_vers_epine_av, THETA_SCAPLOC_vers_epine_mi, THETA_SCAPLOC_vers_epine_ar,... +PHI_SCAPLOC_vers_epine_av, PHI_SCAPLOC_vers_epine_mi, PHI_SCAPLOC_vers_epine_ar,... +O_SCAPLOC_vers_epine_av, O_SCAPLOC_vers_epine_mi, O_SCAPLOC_vers_epine_ar,... +SCAPLOCB_SCAPLOC, SCAPLOCLM_SCAPLOC,SCAPLOCMM_SCAPLOC ); + + + + + + + +end \ No newline at end of file diff --git a/Functions/Calibration/Scapula/ConvertClustertoScaplocBilinear.m b/Functions/Calibration/Scapula/ConvertClustertoScaplocBilinear.m index cba9f7d0..22c1907e 100644 --- a/Functions/Calibration/Scapula/ConvertClustertoScaplocBilinear.m +++ b/Functions/Calibration/Scapula/ConvertClustertoScaplocBilinear.m @@ -1,77 +1,315 @@ -function [new_name,p_trial]=ConvertClustertoScaplocBilinear(filename_trial, filename_arr, filename_av, filename_n, token_mean) - - - [~,p_trial_arr_n,SCAPLOCB_trial_arr_n,SCAPLOCMM_trial_arr_n,SCAPLOCLM_trial_arr_n]=ConvertClustertoScaplocLinear(filename_trial, filename_arr, filename_n, token_mean); - p_trial_arr_n = p_trial_arr_n(:); - %% Arr -> neutre - - - [~,p_trial_n_av,SCAPLOCB_trial_n_av,SCAPLOCMM_trial_n_av,SCAPLOCLM_trial_n_av]=ConvertClustertoScaplocLinear(filename_trial, filename_n, filename_av, token_mean); - - - - - SCAPLOCB_trial = SCAPLOCB_trial_arr_n; - SCAPLOCB_trial(p_trial_arr_n>1,:) = SCAPLOCB_trial_n_av(p_trial_arr_n>1,:); - - - SCAPLOCMM_trial = SCAPLOCMM_trial_arr_n; - SCAPLOCMM_trial(p_trial_arr_n>1,:) = SCAPLOCMM_trial_n_av(p_trial_arr_n>1,:); - - - SCAPLOCLM_trial = SCAPLOCLM_trial_arr_n; - SCAPLOCLM_trial(p_trial_arr_n>1,:) = SCAPLOCLM_trial_n_av(p_trial_arr_n>1,:); - - p_trial = p_trial_arr_n; - p_trial(p_trial_arr_n>1) = p_trial_n_av(p_trial_arr_n>1); - - - - % Open file of trial - h_trial = btkReadAcquisition([char(filename_trial)]); - markers_trial=btkGetMarkers(h_trial); - ListMarkersName_trial = fieldnames(markers_trial); - +function [new_name,p_trial]=ConvertClustertoScaplocBilinear(filename_trial, filename_arr, filename_av, token_mean,side) %% Open files - subject = char(filename_trial(1:6)); - % Open file of trial - h_trial = btkReadAcquisition([char(filename_trial)]); - markers_trial=btkGetMarkers(h_trial); - ListMarkersName_trial = fieldnames(markers_trial); +subject = char(filename_trial(1:6)); +% Open file of trial +h_trial = btkReadAcquisition([char(filename_trial)]); +markers_trial=btkGetMarkers(h_trial); +ListMarkersName_trial = fieldnames(markers_trial); +% Open file for rear pose +h_arr = btkReadAcquisition([char(filename_arr)]); +markers_arr=btkGetMarkers(h_arr); +% Open file for advanced pose +h_av = btkReadAcquisition([char(filename_av)]); +markers_av=btkGetMarkers(h_av); + +%% Get positions of clusters and scaploc in filename arr and filename av +% Trial cluster +names_trial = fieldnames(markers_trial); +prefix_trial = names_trial{contains(names_trial,['SCAP' side 'B'])}; +prefix_trial=prefix_trial(1:end-6); +if strcmp(prefix_trial,['SCAP' side 'B']) + prefix_trial=''; +end + +SCAPDB_trial=markers_trial.([prefix_trial 'SCAP' side 'B']); + +nb_frame = size(SCAPDB_trial,1); +SCAPDB_trial = reshape(SCAPDB_trial, [nb_frame 1 3]); +SCAPDB_trial = permute(SCAPDB_trial, [3,2,1]); + +SCAPDH_trial = markers_trial.([prefix_trial 'SCAP' side 'H']); +SCAPDH_trial = reshape(SCAPDH_trial, [nb_frame 1 3]); +SCAPDH_trial = permute(SCAPDH_trial, [3,2,1]); + +SCAPDL_trial = markers_trial.([prefix_trial 'SCAP' side 'L']); + +SCAPDL_trial = reshape(SCAPDL_trial, [nb_frame 1 3]); +SCAPDL_trial = permute(SCAPDL_trial, [3,2,1]); + +%Thorax trial +C7_trial=markers_trial.([prefix_trial 'C7']); +C7_trial = reshape(C7_trial, [nb_frame 1 3]); +C7_trial = permute(C7_trial, [3,2,1]); +MAN_trial = markers_trial.([prefix_trial 'MAN']); +MAN_trial = reshape(MAN_trial, [nb_frame 1 3]); +MAN_trial = permute(MAN_trial, [3,2,1]); +T8_trial = markers_trial.([prefix_trial 'T8']); +T8_trial = reshape(T8_trial, [nb_frame 1 3]); +T8_trial = permute(T8_trial, [3,2,1]); + +% Rear cluster +names_arr = fieldnames(markers_arr); +prefix_arr = names_arr{contains(names_arr,['SCAP' side 'B'])}; +prefix_arr=prefix_arr(1:end-6); +if strcmp(prefix_arr,['SCAP' side 'B']) + prefix_arr=''; +end +% prefix_arr = '';%prefix_arr(1:6); +SCAPDB_arr=markers_arr.([prefix_arr 'SCAP' side 'B']); +SCAPDH_arr=markers_arr.([prefix_arr 'SCAP' side 'H']); +SCAPDL_arr=markers_arr.([prefix_arr 'SCAP' side 'L']); +%THORAX ARRIERE +C7_arr=markers_arr.([prefix_arr 'C7']); +MAN_arr=markers_arr.([prefix_arr 'MAN']); +T8_arr=markers_arr.([prefix_arr 'T8']); + + +% Rear scaploc +try + prefix_scaploc = names_arr{contains(names_arr,['Scaploc_B' side])}; + prefix_scaploc = prefix_scaploc(1:end-9); +catch + prefix_scaploc=''; +end +try + SCAPLOCB_arr=markers_arr.([prefix_scaploc 'Scaploc_B']); + SCAPLOCMM_arr=markers_arr.([prefix_scaploc 'Scaploc_MM']); + SCAPLOCLM_arr=markers_arr.([prefix_scaploc 'Scaploc_LM']); +catch + SCAPLOCB_arr=markers_arr.([prefix_scaploc 'SCAPLOCB']); + SCAPLOCMM_arr=markers_arr.([prefix_scaploc 'SCAPLOCMM']); + SCAPLOCLM_arr=markers_arr.([prefix_scaploc 'SCAPLOCLM']); +end +% Advanced cluster +names_av = fieldnames(markers_av); +prefix_av = names_av{contains(names_av,['SCAP' side 'B'])}; +prefix_av=prefix_av(1:end-6); +if strcmp(prefix_av,['SCAP' side 'B']) + prefix_av=''; +end +SCAPDB_av=markers_av.([prefix_av 'SCAP' side 'B']); +SCAPDH_av=markers_av.([prefix_av 'SCAP' side 'H']); +SCAPDL_av=markers_av.([prefix_av 'SCAP' side 'L']); + +%THORAX AVANT +C7_av=markers_av.([prefix_av 'C7']); +MAN_av=markers_av.([prefix_av 'MAN']); +T8_av=markers_av.([prefix_av 'T8']); +% Advanced scaploc + +try + SCAPLOCB_av=markers_av.([prefix_scaploc 'Scaploc_B']); + SCAPLOCMM_av=markers_av.([prefix_scaploc 'Scaploc_MM']); + SCAPLOCLM_av=markers_av.([prefix_scaploc 'Scaploc_LM']); +catch + SCAPLOCB_av=markers_av.([prefix_scaploc 'SCAPLOCB']); + SCAPLOCMM_av=markers_av.([prefix_scaploc 'SCAPLOCMM']); + SCAPLOCLM_av=markers_av.([prefix_scaploc 'SCAPLOCLM']); +end + +%% Mean the positions of the markers to reduce noise on static poses +if token_mean==1 + % Rear cluster + SCAPDB_arr=mean(SCAPDB_arr); + SCAPDH_arr=mean(SCAPDH_arr); + SCAPDL_arr=mean(SCAPDL_arr); + %Rear thorax + C7_arr=mean(C7_arr); + MAN_arr=mean(MAN_arr); + T8_arr=mean(T8_arr); + % Rear scaploc + SCAPLOCB_arr=mean(SCAPLOCB_arr); + SCAPLOCMM_arr=mean(SCAPLOCMM_arr); + SCAPLOCLM_arr=mean(SCAPLOCLM_arr); + % Advanced cluster + SCAPDB_av=mean(SCAPDB_av); + SCAPDH_av=mean(SCAPDH_av); + SCAPDL_av=mean(SCAPDL_av); + %Advanced thorax + C7_av=mean(C7_av); + MAN_av=mean(MAN_av); + T8_av=mean(T8_av); + % Advanced scaploc + SCAPLOCB_av=mean(SCAPLOCB_av); + SCAPLOCMM_av=mean(SCAPLOCMM_av); + SCAPLOCLM_av=mean(SCAPLOCLM_av); +else + % Rear cluster + SCAPDB_arr=SCAPDB_arr(1,:); + SCAPDH_arr=SCAPDH_arr(1,:); + SCAPDL_arr=SCAPDL_arr(1,:); + % Rear scaploc + SCAPLOCB_arr=SCAPLOCB_arr(1,:); + SCAPLOCMM_arr=SCAPLOCMM_arr(1,:); + SCAPLOCLM_arr=SCAPLOCLM_arr(1,:); + % Advanced scaploc + SCAPDB_av=SCAPDB_av(1,:); + SCAPDH_av=SCAPDH_av(1,:); + SCAPDL_av=SCAPDL_av(1,:); + % Advanced scaploc + SCAPLOCB_av=SCAPLOCB_av(1,:); + SCAPLOCMM_av=SCAPLOCMM_av(1,:); + SCAPLOCLM_av=SCAPLOCLM_av(1,:); +end + +%% Find coefficients for quaternion interpolation +% Rear cluster homogeneous matrix +O_spine_arr = SCAPDB_arr; +X_spine_arr = (SCAPDL_arr - SCAPDB_arr)/norm(SCAPDL_arr - SCAPDB_arr); +yt_spine_arr = SCAPDH_arr - SCAPDB_arr; +Z_spine_arr = (cross(X_spine_arr,yt_spine_arr))/norm(cross(X_spine_arr,yt_spine_arr)); +Y_spine_arr = cross(Z_spine_arr,X_spine_arr); +T_spine_world_arr = [X_spine_arr' Y_spine_arr' Z_spine_arr' O_spine_arr'; 0 0 0 1]; +% Rear Thorax homogeneous matrix +O_thorax_arr = C7_arr; +X_thorax_arr = (MAN_arr - C7_arr)/norm(MAN_arr - C7_arr); +yt_thorax_arr = T8_arr - C7_arr; +Z_thorax_arr = (cross(X_thorax_arr,yt_thorax_arr))/norm(cross(X_thorax_arr,yt_thorax_arr)); +Y_thorax_arr = cross(Z_thorax_arr,X_thorax_arr); +T_thorax_world_arr = [X_thorax_arr' Y_thorax_arr' Z_thorax_arr' O_thorax_arr'; 0 0 0 1]; + +% Rear scaploc homogeneous matrix +O_SCAPLOC_arr = SCAPLOCB_arr; +X_SCAPLOC_arr = (SCAPLOCLM_arr - SCAPLOCB_arr)/norm(SCAPLOCLM_arr - SCAPLOCB_arr); +yt_SCAPLOC_arr = (SCAPLOCMM_arr - SCAPLOCB_arr)/norm(SCAPLOCMM_arr - SCAPLOCB_arr); +Z_SCAPLOC_arr = cross(X_SCAPLOC_arr,yt_SCAPLOC_arr)/norm(cross(X_SCAPLOC_arr,yt_SCAPLOC_arr)); +Y_SCAPLOC_arr = cross(Z_SCAPLOC_arr,X_SCAPLOC_arr); +T_SCAPLOC_world_arr = [X_SCAPLOC_arr' Y_SCAPLOC_arr' Z_SCAPLOC_arr' O_SCAPLOC_arr'; 0 0 0 1]; +% Scaploc markers in scaploc frame +SCAPLOCMM_SCAPLOC=T_SCAPLOC_world_arr\[SCAPLOCMM_arr';1]; +SCAPLOCLM_SCAPLOC=T_SCAPLOC_world_arr\[SCAPLOCLM_arr';1]; +SCAPLOCB_SCAPLOC=T_SCAPLOC_world_arr\[SCAPLOCB_arr';1]; +SCAPLOCMM_SCAPLOC=SCAPLOCMM_SCAPLOC(1:3)'; +SCAPLOCLM_SCAPLOC=SCAPLOCLM_SCAPLOC(1:3)'; +SCAPLOCB_SCAPLOC=SCAPLOCB_SCAPLOC(1:3)'; +% Rear homogenous matrix between scaploc and cluster +T_SCAPLOC_spine_arr = T_spine_world_arr\T_SCAPLOC_world_arr; +% Distance between scaploc and cluster on rear pose +O_SCAPLOC_spine_arr = T_SCAPLOC_spine_arr(:,4); +O_SCAPLOC_spine_arr = O_SCAPLOC_spine_arr(1:3); + +% Rotation matrix between scaploc and cluster on rear pose +R_SCAPLOC_spine_arr = T_SCAPLOC_spine_arr(1:3,1:3); +% Quaternion between scaploc and cluster on rear pose +Q_SCAPLOC_spine_arr = quaternion(rotm2quat(R_SCAPLOC_spine_arr)); + +% Advanced cluster homogeneous matrix +O_spine_av = SCAPDB_av; +X_spine_av = (SCAPDL_av - SCAPDB_av)/norm(SCAPDL_av - SCAPDB_av); +yt_spine_av = SCAPDH_av - SCAPDB_av; +Z_spine_av = (cross(X_spine_av,yt_spine_av))/norm(cross(X_spine_av,yt_spine_av)); +Y_spine_av = cross(Z_spine_av,X_spine_av); +T_spine_world_av = [X_spine_av' Y_spine_av' Z_spine_av' O_spine_av'; 0 0 0 1]; +% Advanced thorax homogeneous matrix +O_thorax_av = C7_av; +X_thorax_av = (MAN_av - C7_av)/norm(MAN_av - C7_av); +yt_thorax_av = T8_av - C7_av; +Z_thorax_av = (cross(X_thorax_av,yt_thorax_av))/norm(cross(X_thorax_av,yt_thorax_av)); +Y_thorax_av = cross(Z_thorax_av,X_thorax_av); +T_thorax_world_av = [X_thorax_av' Y_thorax_av' Z_thorax_av' O_thorax_av'; 0 0 0 1]; + +% Advanced scaploc homogeneous matrix +O_SCAPLOC_av = SCAPLOCB_av; +X_SCAPLOC_av = (SCAPLOCLM_av - SCAPLOCB_av)/norm(SCAPLOCLM_av - SCAPLOCB_av); +yt_SCAPLOC_av = (SCAPLOCMM_av - SCAPLOCB_av)/norm(SCAPLOCMM_av - SCAPLOCB_av); +Z_SCAPLOC_av = cross(X_SCAPLOC_av,yt_SCAPLOC_av)/norm(cross(X_SCAPLOC_av,yt_SCAPLOC_av)); +Y_SCAPLOC_av = cross(Z_SCAPLOC_av,X_SCAPLOC_av); +T_SCAPLOC_world_av = [X_SCAPLOC_av' Y_SCAPLOC_av' Z_SCAPLOC_av' O_SCAPLOC_av'; 0 0 0 1]; +% Advanced homogenous matrix between scaploc and cluster +T_SCAPLOC_spine_av = T_spine_world_av\T_SCAPLOC_world_av; +% Distance between scaploc and cluster on advance pose +O_SCAPLOC_spine_av = T_SCAPLOC_spine_av(:,4); +O_SCAPLOC_spine_av = O_SCAPLOC_spine_av(1:3); +% Rotation matrix between scaploc and cluster on advanced pose +R_SCAPLOC_spine_av = T_SCAPLOC_spine_av(1:3,1:3); +% Quaternion between scaploc and cluster on advanced pose +Q_SCAPLOC_spine_av = quaternion(rotm2quat(R_SCAPLOC_spine_av)); + +% Trial cluster homogeneous matrix +O_spine_trial = SCAPDB_trial; +X_spine_trial = (SCAPDL_trial - SCAPDB_trial)./vecnorm(SCAPDL_trial - SCAPDB_trial); +yt_spine_trial = SCAPDH_trial - SCAPDB_trial; +Z_spine_trial = (cross(X_spine_trial,yt_spine_trial))./vecnorm(cross(X_spine_trial,yt_spine_trial)); +Y_spine_trial = cross(Z_spine_trial,X_spine_trial); +last_row = zeros(1,4,nb_frame);last_row(:,4,:)=1; +H_spine_world_trial = [X_spine_trial Y_spine_trial Z_spine_trial O_spine_trial]; +H_spine_world_trial(4,:,:)=last_row; + +% Trial thorax homogeneous matrix +O_thorax_trial = C7_trial; +X_thorax_trial = (MAN_trial - C7_trial)./vecnorm(MAN_trial - C7_trial); +yt_thorax_trial = T8_trial - C7_trial; +Z_thorax_trial = (cross(X_thorax_trial,yt_thorax_trial))./vecnorm(cross(X_thorax_trial,yt_thorax_trial)); +Y_thorax_trial = cross(Z_thorax_trial,X_thorax_trial); +last_row = zeros(1,4,nb_frame);last_row(:,4,:)=1; +T_thorax_world_trial = [X_thorax_trial Y_thorax_trial Z_thorax_trial O_thorax_trial]; +T_thorax_world_trial(4,:,:)=last_row; + +%% Apply quaternion interpolation to cluster from filename_trial +O_spine_thorax_arr = T_thorax_world_arr\[O_spine_arr';1]; +O_spine_thorax_arr = O_spine_thorax_arr(1:3); +O_spine_thorax_av = T_thorax_world_av\[O_spine_av';1]; +O_spine_thorax_av = O_spine_thorax_av(1:3); +for i=1:nb_frame + O_spine_thorax_trial(:,:,i) = T_thorax_world_trial(:,:,i)\[O_spine_trial(:,:,i);1]; +end +O_spine_thorax_trial=O_spine_thorax_trial(1:3,:,:); +% Interpolation coefficient based on cluster position +p_trial=vecnorm(O_spine_thorax_arr-O_spine_thorax_trial)./norm(O_spine_thorax_av-O_spine_thorax_arr); +% Slerp interpolation to compute quaternion between virtual scaploc and +% cluster during trial +Q_SCAPLOC_spine_trial = QuatSlerpAtHome(Q_SCAPLOC_spine_arr,Q_SCAPLOC_spine_av,p_trial); +% Rotation matrix between virtual scaploc and cluster +R_SCAPLOC_spine_trial = quat2rotm(Q_SCAPLOC_spine_trial(:)); +% Distance between virtual scaploc and cluster +O_SCAPLOC_spine_trial = (O_SCAPLOC_spine_av-O_SCAPLOC_spine_arr).*p_trial + O_SCAPLOC_spine_arr; +% Homogeneous matrix between virtaul scaploc and cluster +H_SCAPLOC_spine_trial = [R_SCAPLOC_spine_trial O_SCAPLOC_spine_trial]; +H_SCAPLOC_spine_trial(4,:,:)=last_row; +% Homogeneous matrix between virtual scaploc and world +H_SCAPLOC_world_trial = pagemtimes(H_spine_world_trial,H_SCAPLOC_spine_trial); +% Homogeneous matrix between virtual scaploc and thorax +% Computation of virtual scaploc marker positions +SCAPLOCB_trial=pagemtimes(H_SCAPLOC_world_trial,[SCAPLOCB_SCAPLOC';1]); +SCAPLOCLM_trial=pagemtimes(H_SCAPLOC_world_trial,[SCAPLOCLM_SCAPLOC';1]); +SCAPLOCMM_trial=pagemtimes(H_SCAPLOC_world_trial,[SCAPLOCMM_SCAPLOC';1]); +SCAPLOCB_trial=reshape(permute(SCAPLOCB_trial(1:3,:,:),[3,1,2]),[nb_frame 3]); +SCAPLOCLM_trial=reshape(permute(SCAPLOCLM_trial(1:3,:,:),[3,1,2]),[nb_frame 3]); +SCAPLOCMM_trial=reshape(permute(SCAPLOCMM_trial(1:3,:,:),[3,1,2]),[nb_frame 3]); %% Write scaploc in filename_c3d and save it - % Number of markers in trial - pn_trial=btkGetPointNumber(h_trial); - % Adding virtual SCAPLOC markers to list of markers - pn_new=pn_trial+3; - labels=[ListMarkersName_trial; {'SCAPLOCB';'SCAPLOCMM';'SCAPLOCLM'}]; - % Number of frames in trial - nb_frame_trial=btkGetLastFrame(h_trial)-btkGetFirstFrame(h_trial)+1; - % Trial acquisition frequency - f_trial=btkGetPointFrequency(h_trial); - - % Creation of a new c3d file - h_new = btkNewAcquisition(pn_new,nb_frame_trial); - % Setting acquisition frequency - btkSetFrequency(h_new, f_trial); - - % Copying markers from trial - for i=1:pn_trial - btkSetPointLabel(h_new, i, labels{i}); - btkSetPoint(h_new, i, markers_trial.(ListMarkersName_trial{i})); - end - % Defining virtual SCAPLOC markers - % SCAPLOCB - btkSetPointLabel(h_new, pn_new-2, labels{pn_new-2}); - btkSetPoint(h_new, pn_new-2, SCAPLOCB_trial); - % SCAPLOCMM - btkSetPointLabel(h_new, pn_new-1, labels{pn_new-1}); - btkSetPoint(h_new, pn_new-1, SCAPLOCMM_trial); - % SCAPLOCLM - btkSetPointLabel(h_new, pn_new, labels{pn_new}); - btkSetPoint(h_new, pn_new, SCAPLOCLM_trial); - - % Write and save c3d - new_name=[char(filename_trial(1:end-4)) '_scaplocBilinear.c3d']; - btkWriteAcquisition(h_new, new_name); +% Number of markers in trial +pn_trial=btkGetPointNumber(h_trial); +% Adding virtual SCAPLOC markers to list of markers +pn_new=pn_trial+3; +labels=[ListMarkersName_trial; {['SCAPLOCB' side];['SCAPLOCMM' side];['SCAPLOCLM' side]}]; +% Number of frames in trial +nb_frame_trial=btkGetLastFrame(h_trial)-btkGetFirstFrame(h_trial)+1; +% Trial acquisition frequency +f_trial=btkGetPointFrequency(h_trial); + +% Creation of a new c3d file +h_new = btkNewAcquisition(pn_new,nb_frame_trial); +% Setting acquisition frequency +btkSetFrequency(h_new, f_trial); + +% Copying markers from trial +for i=1:pn_trial + btkSetPointLabel(h_new, i, labels{i}); + btkSetPoint(h_new, i, markers_trial.(ListMarkersName_trial{i})); +end +% Defining virtual SCAPLOC markers +% SCAPLOCB +btkSetPointLabel(h_new, pn_new-2, labels{pn_new-2}); +btkSetPoint(h_new, pn_new-2, SCAPLOCB_trial); +% SCAPLOCMM +btkSetPointLabel(h_new, pn_new-1, labels{pn_new-1}); +btkSetPoint(h_new, pn_new-1, SCAPLOCMM_trial); +% SCAPLOCLM +btkSetPointLabel(h_new, pn_new, labels{pn_new}); +btkSetPoint(h_new, pn_new, SCAPLOCLM_trial); + +% Write and save c3d +new_name=[char(filename_trial(1:end-4)) '.c3d']; +btkWriteAcquisition(h_new, new_name); end \ No newline at end of file diff --git a/Functions/Calibration/Scapula/ConvertClustertoScaplocLinear.m b/Functions/Calibration/Scapula/ConvertClustertoScaplocLinear.m index 26d60848..d98c4c16 100644 --- a/Functions/Calibration/Scapula/ConvertClustertoScaplocLinear.m +++ b/Functions/Calibration/Scapula/ConvertClustertoScaplocLinear.m @@ -17,14 +17,19 @@ names_trial = fieldnames(markers_trial); prefix_trial = names_trial{contains(names_trial,'MTACDB')}; prefix_trial = prefix_trial(1:6); - SCAPDB_trial=markers_trial.([prefix_trial '_MTACDB']); + if strcmp(prefix_trial,'MTACDB') + prefix_trial=''; + else + prefix_trial = [prefix_trial '_']; + end + SCAPDB_trial=markers_trial.([prefix_trial 'MTACDB']); nb_frame = size(SCAPDB_trial,1); SCAPDB_trial = reshape(SCAPDB_trial, [nb_frame 1 3]); SCAPDB_trial = permute(SCAPDB_trial, [3,2,1]); - SCAPDH_trial = markers_trial.([prefix_trial '_MTACDM']); + SCAPDH_trial = markers_trial.([prefix_trial 'MTACDM']); SCAPDH_trial = reshape(SCAPDH_trial, [nb_frame 1 3]); SCAPDH_trial = permute(SCAPDH_trial, [3,2,1]); - SCAPDL_trial = markers_trial.([prefix_trial '_MTACDL']); + SCAPDL_trial = markers_trial.([prefix_trial 'MTACDL']); SCAPDL_trial = reshape(SCAPDL_trial, [nb_frame 1 3]); SCAPDL_trial = permute(SCAPDL_trial, [3,2,1]); @@ -32,9 +37,14 @@ names_arr = fieldnames(markers_arr); prefix_arr = names_arr{contains(names_arr,'MTACDB')}; prefix_arr = prefix_arr(1:6); - SCAPDB_arr=markers_arr.([prefix_arr '_MTACDB']); - SCAPDH_arr=markers_arr.([prefix_arr '_MTACDM']); - SCAPDL_arr=markers_arr.([prefix_arr '_MTACDL']); + if strcmp(prefix_arr,'MTACDB') + prefix_arr=''; + else + prefix_arr = [prefix_arr '_']; + end + SCAPDB_arr=markers_arr.([prefix_arr 'MTACDB']); + SCAPDH_arr=markers_arr.([prefix_arr 'MTACDM']); + SCAPDL_arr=markers_arr.([prefix_arr 'MTACDL']); % Rear scaploc SCAPLOCB_arr=markers_arr.('ScapLoc_SCLB'); SCAPLOCMM_arr=markers_arr.('ScapLoc_SCLM'); @@ -44,9 +54,14 @@ names_av = fieldnames(markers_av); prefix_av = names_av{contains(names_av,'MTACDB')}; prefix_av = prefix_av(1:6); - SCAPDB_av=markers_av.([prefix_av '_MTACDB']); - SCAPDH_av=markers_av.([prefix_av '_MTACDM']); - SCAPDL_av=markers_av.([prefix_av '_MTACDL']); + if strcmp(prefix_av,'MTACDB') + prefix_av=''; + else + prefix_av = [prefix_av '_']; + end + SCAPDB_av=markers_av.([prefix_av 'MTACDB']); + SCAPDH_av=markers_av.([prefix_av 'MTACDM']); + SCAPDL_av=markers_av.([prefix_av 'MTACDL']); % Advanced scaploc SCAPLOCB_av=markers_av.('ScapLoc_SCLB'); SCAPLOCMM_av=markers_av.('ScapLoc_SCLM'); diff --git a/Functions/Calibration/Scapula/ConvertClustertoScaplocRigid.m b/Functions/Calibration/Scapula/ConvertClustertoScaplocRigid.m index b814d62e..8b88bd7b 100644 --- a/Functions/Calibration/Scapula/ConvertClustertoScaplocRigid.m +++ b/Functions/Calibration/Scapula/ConvertClustertoScaplocRigid.m @@ -15,14 +15,19 @@ names_trial = fieldnames(markers_trial); prefix_trial = names_trial{contains(names_trial,'MTACDB')}; prefix_trial = prefix_trial(1:6); - SCAPDB_trial=markers_trial.([prefix_trial '_MTACDB']); + if strcmp(prefix_trial,'MTACDB') + prefix_trial=''; + else + prefix_trial = [prefix_trial '_']; + end + SCAPDB_trial=markers_trial.([prefix_trial 'MTACDB']); nb_frame = size(SCAPDB_trial,1); SCAPDB_trial = reshape(SCAPDB_trial, [nb_frame 1 3]); SCAPDB_trial = permute(SCAPDB_trial, [3,2,1]); - SCAPDH_trial = markers_trial.([prefix_trial '_MTACDM']); + SCAPDH_trial = markers_trial.([prefix_trial 'MTACDM']); SCAPDH_trial = reshape(SCAPDH_trial, [nb_frame 1 3]); SCAPDH_trial = permute(SCAPDH_trial, [3,2,1]); - SCAPDL_trial = markers_trial.([prefix_trial '_MTACDL']); + SCAPDL_trial = markers_trial.([prefix_trial 'MTACDL']); SCAPDL_trial = reshape(SCAPDL_trial, [nb_frame 1 3]); SCAPDL_trial = permute(SCAPDL_trial, [3,2,1]); @@ -30,9 +35,14 @@ names_ne = fieldnames(markers_ne); prefix_ne = names_ne{contains(names_ne,'MTACDB')}; prefix_ne = prefix_ne(1:6); - SCAPDB_ne=markers_ne.([prefix_ne '_MTACDB']); - SCAPDH_ne=markers_ne.([prefix_ne '_MTACDM']); - SCAPDL_ne=markers_ne.([prefix_ne '_MTACDL']); + if strcmp(prefix_ne,'MTACDB') + prefix_ne=''; + else + prefix_ne = [prefix_ne '_']; + end + SCAPDB_ne=markers_ne.([prefix_ne 'MTACDB']); + SCAPDH_ne=markers_ne.([prefix_ne 'MTACDM']); + SCAPDL_ne=markers_ne.([prefix_ne 'MTACDL']); % Neutral scaploc SCAPLOCB_ne=markers_ne.('ScapLoc_SCLB'); SCAPLOCMM_ne=markers_ne.('ScapLoc_SCLM'); diff --git a/Functions/DataImport/Get_real_markers.m b/Functions/DataImport/Get_real_markers.m index 1ca236a0..2ca2852e 100644 --- a/Functions/DataImport/Get_real_markers.m +++ b/Functions/DataImport/Get_real_markers.m @@ -147,6 +147,7 @@ % Filtrage (Filtering) if AnalysisParameters.General.FilterActive for i=1:numel(real_markers) + real_markers(i).position_c3d(isnan(real_markers(i).position_c3d))=0; real_markers(i).position = filt_data(real_markers(i).position_c3d,AnalysisParameters.General.FilterCutOff,f); end else diff --git a/Functions/DataImport/InputDataImport/C3dProcessedData.m b/Functions/DataImport/InputDataImport/C3dProcessedData.m index bf6f5181..8504333d 100644 --- a/Functions/DataImport/InputDataImport/C3dProcessedData.m +++ b/Functions/DataImport/InputDataImport/C3dProcessedData.m @@ -112,10 +112,20 @@ if isempty(find(markers.(list_marker_c3d{ii})==0,1)) real_markers(cpt).name=list_m_table(Ia(ii)); real_markers(cpt).position_c3d=markers.(list_marker_c3d{ii})/1000; + if ~sum(isnan(real_markers(cpt).position_c3d)) + real_markers(cpt).weight = 1; + else + real_markers(cpt).weight = 0; + end + cpt = cpt+1; else warning(['A least one marker is occluded in ' filename ', occluded markers :']) disp(list_m_table(Ia(ii))); + real_markers(cpt).name=list_m_table(Ia(ii)); + real_markers(cpt).position_c3d=zeros(nb_frame,3); + real_markers(cpt).weight = 0; + cpt = cpt+1; end end diff --git a/Functions/Dynamics/CalcMC.m b/Functions/Dynamics/CalcMC.m index 2ad0d1d7..356cd556 100644 --- a/Functions/Dynamics/CalcMC.m +++ b/Functions/Dynamics/CalcMC.m @@ -18,6 +18,6 @@ if j==0 mc=0; else - mc=Human_model(j).m*(Human_model(j).p+Human_model(j).R*Human_model(j).c); + mc=Human_model(j).m.*(Human_model(j).p+Human_model(j).R.*Human_model(j).c); mc=mc+CalcMC(Human_model,Human_model(j).sister)+CalcMC(Human_model,Human_model(j).child); end \ No newline at end of file diff --git a/Functions/Dynamics/InverseDynamics.m b/Functions/Dynamics/InverseDynamics.m index 5fb2132a..cbd61cbf 100644 --- a/Functions/Dynamics/InverseDynamics.m +++ b/Functions/Dynamics/InverseDynamics.m @@ -67,11 +67,11 @@ %% articular speed and acceleration dt=1/freq; - dq=derivee2(dt,q); % vitesses - ddq=derivee2(dt,dq); % acc�l�rations - + dq=InverseKinematicsResults.JointCoordinatesFirstDerivative' ; % vitesses + ddq=InverseKinematicsResults.JointCoordinatesSecondDerivative' ; % acc�l�rations nbframe=size(q,1); + %% D�finition des donn�es cin�matiques du pelvis % (position / vitesse / acc�l�ration / orientation / vitesse angulaire / acc�l�ration angulaire) % Kinematical data for Pelvis (Position/speed/acceleration/angles/angular speed/angular acceleration) diff --git a/Functions/ExternalForces/ExternalForcesComputation.m b/Functions/ExternalForces/ExternalForcesComputation.m index f31cb944..32fc79c7 100644 --- a/Functions/ExternalForces/ExternalForcesComputation.m +++ b/Functions/ExternalForces/ExternalForcesComputation.m @@ -30,7 +30,7 @@ load('BiomechanicalModel.mat'); %#ok end -parfor i = 1:numel(AnalysisParameters.filename) +for i = 1:numel(AnalysisParameters.filename) filename = AnalysisParameters.filename{i}(1:end-(numel(AnalysisParameters.General.Extension)-1)); if AnalysisParameters.ID.InputData == 0 [ExternalForcesComputationResults] = ExternalForces_Zero(filename, BiomechanicalModel); diff --git a/Functions/ExternalForces/FromExperiments/Wheelchair.m b/Functions/ExternalForces/FromExperiments/Wheelchair.m new file mode 100644 index 00000000..303d9d7d --- /dev/null +++ b/Functions/ExternalForces/FromExperiments/Wheelchair.m @@ -0,0 +1,111 @@ +function [ExternalForcesComputationResults] = Wheelchair(filename, BiomechanicalModel, AnalysisParameters) +% Computation of the external forces for a cycling application +% +% INPUT +% - filename: name of the file to process (character string) +% - BiomechanicalModel: musculoskeletal model +% - AnalysisParameters: parameters of the musculoskeletal analysis, +% automatically generated by the graphic interface 'Analysis' +% OUTPUT +% - ExternalForcesComputationResults: results of the external forces +% computation (see the Documentation for the structure) +%________________________________________________________ +% +% Licence +% Toolbox distributed under GPL 3.0 Licence +%________________________________________________________ +% +% Authors : Antoine Muller, Charles Pontonnier, Pierre Puchaud and +% Georges Dumont +%________________________________________________________ + +Human_model = BiomechanicalModel.OsteoArticularModel; +load([filename '/ExperimentalData.mat']); %#ok +time = ExperimentalData.Time; +nb_frame=numel(time); +f_mocap=1/time(2); + +% Initialisation +for f=1:nb_frame + for n=1:numel(Human_model) + external_forces(f).fext(n).fext=zeros(3,2); %#ok + end +end + +% Handrim forces +load([filename '.mat']); %#ok +Right_Handrim = FRET.MechAct.RD; +Left_Handrim = FRET.MechAct.RG; +f_wheelchair = FRET.Frequence; +Right_Handrim = resample(Right_Handrim,f_mocap,f_wheelchair); +Left_Handrim = resample(Left_Handrim,f_mocap,f_wheelchair); + +% Filtrage des données (data filtering) +if AnalysisParameters.ExternalForces.FilterActive + for i=1:size(Right_Handrim,2) + Right_Handrim(:,i)=filt_data(Right_Handrim(:,i),AnalysisParameters.ExternalForces.FilterCutOff,f_mocap); + end + for i=1:size(Left_Handrim,2) + Left_Handrim(:,i)=filt_data(Left_Handrim(:,i),AnalysisParameters.ExternalForces.FilterCutOff,f_mocap); + end +end + +% Wheelchair frame +list_markers_wheelchair={'FRMA';'FRMRG';'FRMRD';'FRMAVG';'FRMAVD';'MC2G';'MC5G';'MC2D';'MC5D'}; +[real_markers]=C3dProcessedData(filename, list_markers_wheelchair); + + +for i=1:numel(list_markers_wheelchair) % finding number of marker + for j=1:numel(real_markers) + if strcmp(list_markers_wheelchair{i},real_markers(j).name) + list_markers_wheelchair{i,2}=j; + end + end +end + +%% Handrim forces are added in variable external_forces +% Wheelchair frame computation based on marker position +A = cell(nb_frame,1); +pWheelchair = cell(nb_frame,1); +xWheelchair = cell(nb_frame,1); +yWheelchair = cell(nb_frame,1); +zWheelchair = cell(nb_frame,1); +RWheelchair = cell(nb_frame,1); +RCapteur_fauteuil =[ + 0.9696 0.2444 0 + -0.2444 0.9696 0 + 0 0.0122 1.0000]; +for i=1:nb_frame + % Center of wheelchair frame (midpoint between right and left wheel) + A{i} =(real_markers(list_markers_wheelchair{2,2}).position_c3d(i,:)+real_markers(list_markers_wheelchair{3,2}).position_c3d(i,:))/2; + pWheelchair{i} =A{i}'; + % Wheelchair frame z-axis : FRMRD-FRMRG + zWheelchair_i =(real_markers(list_markers_wheelchair{3,2}).position_c3d(i,:)-real_markers(list_markers_wheelchair{2,2}).position_c3d(i,:)); + zWheelchair{i} =zWheelchair_i/norm(zWheelchair_i); + % FRMAVD - FRMRD + xWheelchair_i = (real_markers(list_markers_wheelchair{5,2}).position_c3d(i,:)-real_markers(list_markers_wheelchair{3,2}).position_c3d(i,:)); + xWheelchair{i} = xWheelchair_i/norm(xWheelchair_i); + yWheelchair{i} = cross(zWheelchair{i},xWheelchair{i})/norm(cross(zWheelchair{i},xWheelchair{i})); + xWheelchair{i} = cross(yWheelchair{i},zWheelchair{i})/norm( cross(yWheelchair{i},zWheelchair{i})); + RWheelchair{i} = [xWheelchair{i}' yWheelchair{i}' zWheelchair{i}']; + RWheelchair{i} = RWheelchair{i}*RCapteur_fauteuil; + COP_Right(i,:) = (RWheelchair{i}'*((real_markers(list_markers_wheelchair{9,2}).position_c3d(i,:)+real_markers(list_markers_wheelchair{8,2}).position_c3d(i,:))/2)')'-A{i}; + COP_Left(i,:) = (RWheelchair{i}'*((real_markers(list_markers_wheelchair{7,2}).position_c3d(i,:)+real_markers(list_markers_wheelchair{6,2}).position_c3d(i,:))/2)')'-A{i}; +end + +% Right Handrim forces +Solid_name='RHand'; +Solid=find(strcmp({Human_model.name},Solid_name)); +[external_forces] = addPlatformForces(external_forces, Solid, pWheelchair, RWheelchair, -[Right_Handrim],COP_Right); + +% Left Handrim forces +Solid_name='LHand'; +Solid=find(strcmp({Human_model.name},Solid_name)); +[external_forces] = addPlatformForces(external_forces, Solid, pWheelchair, RWheelchair, -[Left_Handrim], COP_Left); +% Sauvegarde des données +if exist([filename '/ExternalForcesComputationResults.mat'],'file') + load([filename '/ExternalForcesComputationResults.mat']); +end +ExternalForcesComputationResults.ExternalForcesExperiments = external_forces; + +end diff --git a/Functions/Interface/GenerateParameters.mlapp b/Functions/Interface/GenerateParameters.mlapp index af7269c2..e4f9d67a 100644 Binary files a/Functions/Interface/GenerateParameters.mlapp and b/Functions/Interface/GenerateParameters.mlapp differ diff --git a/Functions/Kinematics/AddClosedLoopEquations.m b/Functions/Kinematics/AddClosedLoopEquations.m index da6bfc75..053092dc 100644 --- a/Functions/Kinematics/AddClosedLoopEquations.m +++ b/Functions/Kinematics/AddClosedLoopEquations.m @@ -8,7 +8,7 @@ % % OUTPUT % - BiomechanicalModel: musculoskeletal model -%________________________________________________________ +%_______________________________________________________ % % Licence % Toolbox distributed under GPL 3.0 Licence @@ -18,6 +18,10 @@ % Georges Dumont %________________________________________________________ +if ~isempty(varargin) + radius=varargin{1}; +end + HumanModel = BiomechanicalModel.OsteoArticularModel; syms q qred real; @@ -30,12 +34,12 @@ ConstraintEq =[]; %% Find constraint equations for closing the loop if size(solid_path1)==[1 1] - [~,ConstraintEq]=NonLinCon_ClosedLoop_Sym(HumanModel,solid_path1{1},solid_path2{1},num_solid,num_markers,q,ones(1,length(q))); + [~,ConstraintEq]=NonLinCon_ClosedLoop_Sym(HumanModel,solid_path1{1},solid_path2{1},num_solid,num_markers,q,ones(1,length(q)),radius); ConstraintEq=ConstraintEq'; else ConstraintEq =[]; for idx= 1:length(num_solid) - [~,ConstraintEq1]=NonLinCon_ClosedLoop_Sym(HumanModel,solid_path1{idx},solid_path2{idx},num_solid(idx),num_markers(idx),q,ones(1,length(q))); + [~,ConstraintEq1]=NonLinCon_ClosedLoop_Sym(HumanModel,solid_path1{idx},solid_path2{idx},num_solid(idx),num_markers(idx),q,ones(1,length(q)),radius); ConstraintEq = [ConstraintEq ConstraintEq1']; end end @@ -93,9 +97,9 @@ Karti = Kartisym(oneqset); % User choice - if ~isempty(varargin) - qinddep = varargin{1}; - end + % if ~isempty(varargin) + % qinddep = varargin{1}; + % end if length(qinddep) < size(Karti,2) - rank(Karti) starting_columns = unique(col); diff --git a/Functions/Kinematics/ConstraintProjectionDerivative.m b/Functions/Kinematics/ConstraintProjectionDerivative.m new file mode 100644 index 00000000..ac49b2dc --- /dev/null +++ b/Functions/Kinematics/ConstraintProjectionDerivative.m @@ -0,0 +1,411 @@ +function [ceq]=ConstraintProjectionDerivative(Human_model,solid_path1,solid_path2,num_solid,num_markers,q,k,qchoix,String) +% Non-linear equation used in the inverse kinematics step for closed loops +% +% INPUT +% - Human_model: osteo-articular model (see the Documentation for the +% structure) +% - solid_path1 : vector of one of the two paths to close the loop +% - solid_path2 : vector of the other of the two paths to close the loop +% - num_solid : vector of the number of solid where the anatomical point must join the +% origin of another joint to close the loop +% - num_markers : vector of the position in the list "anat_position" that +% corresponds to the point to close the loop +% - q: vector of joint coordinates at a given instant +% - k: vector of homothety coefficient +% +% OUTPUT +% - c: non-linar inequality +% - ceq: non-linear equality +%________________________________________________________ +% +% Licence +% Toolbox distributed under GPL 3.0 Licence +%________________________________________________________ +% +% Authors : Antoine Muller, Charles Pontonnier, Pierre Puchaud and +% Georges Dumont +%________________________________________________________ + +% Contraints initialization + +%ceq=zeros(9*length(num_solid),1); + +if strcmp(String,"all") + ceq=zeros(6*length(num_solid),6); +else + ceq=zeros(6*length(num_solid),1); +end + + +for pp=1:numel(num_solid) + + if isempty(solid_path2{pp}) % if the beginning coincides with the end of the loop + + % Computation on path + path = solid_path1{pp}; + ind = find(path==qchoix); + + if (~isempty(ind) && ind~=1) + + [Human_model,p,R] = ForwardKinematics_ClosedLoop(Human_model,1,[0 0 0]',path(1:ind),[0 0 0]',eye(3),q,k); + + + % if strcmp(String,"all") + % + % for idx=1:3 %Translation + % vec=[0 0 0]'; + % vec(idx)=1; + % ptemp = R*vec; + % + % ceq(1+6*(pp-1),idx)=0; + % ceq(2+6*(pp-1),idx)=0; + % ceq(3+6*(pp-1),idx)=0; + % + % ceq(4+6*(pp-1),idx)=ptemp(1); + % ceq(5+6*(pp-1),idx)=ptemp(2); + % ceq(6+6*(pp-1),idx)=ptemp(3); + % end + % + % for idx=1:3 %Rotation + % vec=[0 0 0]'; + % vec(idx)=1; + % s = Human_model(num_solid(pp)).c + Human_model(num_solid(pp)).anat_position{num_markers(pp),2}; % position with respects to the position of the mother solid joint of the closed loop + % Rtemp = R*wedge(vec); + % ptemp = 0*p; + % + % if ~isempty(mainpath(ind+1:end)) + % [Human_model,ptemp,Rtemp] = ForwardKinematics_ClosedLoop(Human_model,1,0*s,mainpath(ind+1:end),0*p,Rtemp,q,k); + % end + % if ~isempty(intersect(mainpath,num_solid(pp))) % Finding the solid with the anatomical position to be respected + % ptemp = Rtemp*s ; + % end + % ceq(1+6*(pp-1),idx+3)=Rtemp(1,1); + % ceq(2+6*(pp-1),idx+3)=Rtemp(2,2); + % ceq(3+6*(pp-1),idx+3)=Rtemp(3,3); + % + % ceq(4+6*(pp-1),idx+3)=ptemp(1); + % ceq(5+6*(pp-1),idx+3)=ptemp(2); + % ceq(6+6*(pp-1),idx+3)=ptemp(3); + % end + % + % else + + if Human_model(qchoix).joint==1 + + + s = Human_model(num_solid(pp)).c + Human_model(num_solid(pp)).anat_position{num_markers(pp),2}; % position with respects to the position of the mother solid joint of the closed loop + Rtemp = R*wedge(Human_model(qchoix).a)*wedge(Human_model(qchoix).a); + ptemp = 0*p; + + if ~isempty(mainpath(ind+1:end)) + [Human_model,ptemp,Rtemp] = ForwardKinematics_ClosedLoop(Human_model,1,0*s,mainpath(ind+1:end),0*p,Rtemp,q,k); + end + if ~isempty(intersect(mainpath,num_solid(pp))) % Finding the solid with the anatomical position to be respected + ptemp = Rtemp*s ; + end + + + ceq(1+6*(pp-1))=Rtemp(1,1); + ceq(2+6*(pp-1))=Rtemp(2,2); + ceq(3+6*(pp-1))=Rtemp(3,3); + % ceq(4+9*(pp-1))=Rtemp(1,2); + % ceq(5+9*(pp-1))=Rtemp(1,3); + % ceq(6+9*(pp-1))=Rtemp(2,3); + ceq(4+6*(pp-1))=ptemp(1); + ceq(5+6*(pp-1))=ptemp(2); + ceq(6+6*(pp-1))=ptemp(3); + + else + ptemp = R*Human_model(qchoix).a; + + ceq(1+6*(pp-1))=0; + ceq(2+6*(pp-1))=0; + ceq(3+6*(pp-1))=0; + % ceq(4+9*(pp-1))=0; + % ceq(5+9*(pp-1))=0; + % ceq(6+9*(pp-1))=0; + ceq(4+6*(pp-1))=0; + ceq(5+6*(pp-1))=0; + ceq(6+6*(pp-1))=0; + + end + + %end + + + + + end + + else + if isempty(solid_path1{pp}) % if the beginning coincides with the end of the loop + + + % Computation on path + path = solid_path2{pp}; + ind = find(path==qchoix); + + if (~isempty(ind) && ind~=1) + + [Human_model,p,R] = ForwardKinematics_ClosedLoop(Human_model,1,[0 0 0]',path(1:ind),[0 0 0]',eye(3),q,k); + + +% if strcmp(String,"all") +% +% for idx=1:3 %Translation +% vec=[0 0 0]'; +% vec(idx)=1; +% ptemp = R*vec; +% +% ceq(1+6*(pp-1),idx)=0; +% ceq(2+6*(pp-1),idx)=0; +% ceq(3+6*(pp-1),idx)=0; +% +% ceq(4+6*(pp-1),idx)=ptemp(1); +% ceq(5+6*(pp-1),idx)=ptemp(2); +% ceq(6+6*(pp-1),idx)=ptemp(3); +% end +% +% for idx=1:3 %Rotation +% vec=[0 0 0]'; +% vec(idx)=1; +% +% s = Human_model(num_solid(pp)).c + Human_model(num_solid(pp)).anat_position{num_markers(pp),2}; % position with respects to the position of the mother solid joint of the closed loop +% Rtemp = R*wedge(vec); +% ptemp = 0*p; +% +% if ~isempty(mainpath(ind+1:end)) +% [Human_model,ptemp,Rtemp] = ForwardKinematics_ClosedLoop(Human_model,1,0*s,mainpath(ind+1:end),0*p,Rtemp,q,k); +% end +% if ~isempty(intersect(mainpath,num_solid(pp))) % Finding the solid with the anatomical position to be respected +% ptemp = Rtemp*s ; +% end +% +% +% ceq(1+6*(pp-1),idx+3)=Rtemp(1,1); +% ceq(2+6*(pp-1),idx+3)=Rtemp(2,2); +% ceq(3+6*(pp-1),idx+3)=Rtemp(3,3); +% +% ceq(4+6*(pp-1),idx+3)=ptemp(1); +% ceq(5+6*(pp-1),idx+3)=ptemp(2); +% ceq(6+6*(pp-1),idx+3)=ptemp(3); +% end +% +% else + + if Human_model(qchoix).joint==1 + + + s = Human_model(num_solid(pp)).c + Human_model(num_solid(pp)).anat_position{num_markers(pp),2}; % position with respects to the position of the mother solid joint of the closed loop + Rtemp = R*wedge(Human_model(qchoix).a)*wedge(Human_model(qchoix).a); + ptemp = 0*p; + + if ~isempty(mainpath(ind+1:end)) + [Human_model,ptemp,Rtemp] = ForwardKinematics_ClosedLoop(Human_model,1,0*s,mainpath(ind+1:end),0*p,Rtemp,q,k); + end + if ~isempty(intersect(mainpath,num_solid(pp))) % Finding the solid with the anatomical position to be respected + ptemp = Rtemp*s ; + end + + + ceq(1+6*(pp-1))=Rtemp(1,1); + ceq(2+6*(pp-1))=Rtemp(2,2); + ceq(3+6*(pp-1))=Rtemp(3,3); + % ceq(4+9*(pp-1))=Rtemp(1,2); + % ceq(5+9*(pp-1))=Rtemp(1,3); + % ceq(6+9*(pp-1))=Rtemp(2,3); + ceq(4+6*(pp-1))=ptemp(1); + ceq(5+6*(pp-1))=ptemp(2); + ceq(6+6*(pp-1))=ptemp(3); + + else + ptemp = R*Human_model(qchoix).a; + + ceq(1+6*(pp-1))=0; + ceq(2+6*(pp-1))=0; + ceq(3+6*(pp-1))=0; + % ceq(4+9*(pp-1))=0; + % ceq(5+9*(pp-1))=0; + % ceq(6+9*(pp-1))=0; + ceq(4+6*(pp-1))=0; + ceq(5+6*(pp-1))=0; + ceq(6+6*(pp-1))=0; + + end + %end + + + + end + + + + else% if the loop is cut elsewhere in the loop + + % Computation on path + path1 = solid_path1{pp}; + ind1 = find(path1==qchoix); + + path2 = solid_path2{pp}; + ind2 = find(path2==qchoix); + + ind=[]; + + if ~isempty(ind1) + mainpath = path1; + secondarypath = path2; + ind = ind1; + + elseif ~isempty(ind2) + mainpath = path2; + secondarypath = path1; + ind = ind2; + + end + + if (~isempty(ind) && ind~=1) + + + + [Human_model,p,R] = ForwardKinematics_ClosedLoop(Human_model,1,[0 0 0]',mainpath(1:ind),[0 0 0]',eye(3),q,k); + [Human_model,~,R_ClosedLoop2] = ForwardKinematics_ClosedLoop(Human_model,1,[0 0 0],secondarypath,[0 0 0]',eye(3),q,k); + + + +% if strcmp(String,"all") +% +% for idx=1:3 %Translation +% vec=[0 0 0]'; +% vec(idx)=1; +% +% ptemp = R*vec; +% +% +% if ~isempty(ind1) +% +% +% ceq(1+6*(pp-1),idx)=0; +% ceq(2+6*(pp-1),idx)=0; +% ceq(3+6*(pp-1),idx)=0; +% +% ceq(4+6*(pp-1),idx)=-ptemp(1); +% ceq(5+6*(pp-1),idx)=-ptemp(2); +% ceq(6+6*(pp-1),idx)=-ptemp(3); +% +% else +% +% ceq(1+6*(pp-1),idx)=0; +% ceq(2+6*(pp-1),idx)=0; +% ceq(3+6*(pp-1),idx)=0; +% +% ceq(4+6*(pp-1),idx)=ptemp(1); +% ceq(5+6*(pp-1),idx)=ptemp(2); +% ceq(6+6*(pp-1),idx)=ptemp(3); +% +% end +% end +% +% for idx=1:3 %Rotation +% vec=[0 0 0]'; +% vec(idx)=1; +% +% s = Human_model(num_solid(pp)).c + Human_model(num_solid(pp)).anat_position{num_markers(pp),2}; % position with respects to the position of the mother solid joint of the closed loop +% Rtemp = R*wedge(vec); +% ptemp = 0*p; +% +% if ~isempty(mainpath(ind+1:end)) +% [Human_model,ptemp,Rtemp] = ForwardKinematics_ClosedLoop(Human_model,1,0*s,mainpath(ind+1:end),0*p,Rtemp,q,k); +% end +% if ~isempty(intersect(mainpath,num_solid(pp))) % Finding the solid with the anatomical position to be respected +% ptemp = Rtemp*s ; +% end +% +% +% if ~isempty(ind1) +% Rtemp= Rtemp*R_ClosedLoop2'; +% +% ceq(1+6*(pp-1),idx+3)=Rtemp(1,1); +% ceq(2+6*(pp-1),idx+3)=Rtemp(2,2); +% ceq(3+6*(pp-1),idx+3)=Rtemp(3,3); +% +% ceq(4+6*(pp-1),idx+3)=-ptemp(1); +% ceq(5+6*(pp-1),idx+3)=-ptemp(2); +% ceq(6+6*(pp-1),idx+3)=-ptemp(3); +% +% else +% +% Rtemp= -Rtemp*R_ClosedLoop2'; +% +% +% ceq(1+6*(pp-1),idx+3)=Rtemp(1,1); +% ceq(2+6*(pp-1),idx+3)=Rtemp(2,2); +% ceq(3+6*(pp-1),idx+3)=Rtemp(3,3); +% +% ceq(4+6*(pp-1),idx+3)=ptemp(1); +% ceq(5+6*(pp-1),idx+3)=ptemp(2); +% ceq(6+6*(pp-1),idx+3)=ptemp(3); +% end +% +% end +% +% else + + if Human_model(qchoix).joint==1 + s = Human_model(num_solid(pp)).c + Human_model(num_solid(pp)).anat_position{num_markers(pp),2}; % position with respects to the position of the mother solid joint of the closed loop + Rtemp = R*wedge(Human_model(qchoix).a)*wedge(Human_model(qchoix).a); + ptemp = 0*p; + + if ~isempty(mainpath(ind+1:end)) + [Human_model,ptemp,Rtemp] = ForwardKinematics_ClosedLoop(Human_model,1,0*s,mainpath(ind+1:end),0*p,Rtemp,q,k); + end + if ~isempty(intersect(mainpath,num_solid(pp))) % Finding the solid with the anatomical position to be respected + ptemp = Rtemp*s ; + end + + if ~isempty(ind1) + Rtemp= Rtemp*R_ClosedLoop2'; + + ceq(1+6*(pp-1))=Rtemp(1,1); + ceq(2+6*(pp-1))=Rtemp(2,2); + ceq(3+6*(pp-1))=Rtemp(3,3); + % ceq(4+9*(pp-1))=Rtemp(1,2); + % ceq(5+9*(pp-1))=Rtemp(1,3); + % ceq(6+9*(pp-1))=Rtemp(2,3); + ceq(4+6*(pp-1))=-ptemp(1); + ceq(5+6*(pp-1))=-ptemp(2); + ceq(6+6*(pp-1))=-ptemp(3); + else + Rtemp= -Rtemp*R_ClosedLoop2'; + ceq(1+6*(pp-1))=Rtemp(1,1); + ceq(2+6*(pp-1))=Rtemp(2,2); + ceq(3+6*(pp-1))=Rtemp(3,3); + % ceq(4+9*(pp-1))=Rtemp(1,2); + % ceq(5+9*(pp-1))=Rtemp(1,3); + % ceq(6+9*(pp-1))=Rtemp(2,3); + ceq(4+6*(pp-1))=ptemp(1); + ceq(5+6*(pp-1))=ptemp(2); + ceq(6+6*(pp-1))=ptemp(3); + + end + else + ptemp = R*Human_model(qchoix).a; + + ceq(1+6*(pp-1))=0; + ceq(2+6*(pp-1))=0; + ceq(3+6*(pp-1))=0; + ceq(4+9*(pp-1))=0; + ceq(5+9*(pp-1))=0; + ceq(6+9*(pp-1))=0; + + + end + %end + + + end + end + end +end + + +end \ No newline at end of file diff --git a/Functions/Kinematics/ConstraintsJacobian.m b/Functions/Kinematics/ConstraintsJacobian.m index 836e6a44..fd897006 100644 --- a/Functions/Kinematics/ConstraintsJacobian.m +++ b/Functions/Kinematics/ConstraintsJacobian.m @@ -1,4 +1,4 @@ -function K=ConstraintsJacobian(BiomechanicalModel,q,solid_path1,solid_path2,num_solid,num_markers,k,dq,dependancies) +function K=ConstraintsJacobian(Human_model,q,solid_path1,solid_path2,num_solid,num_markers,dependancies) % Return the constraint matrix K, which is the jacobian of the constraints % by q @@ -28,41 +28,103 @@ % Georges Dumont %________________________________________________________ + +% for qchoix=1:length(q) +% % qp=q; +% % qm=q; +% % qp(qchoix)=qp(qchoix)+dq; +% % qm(qchoix)=qm(qchoix)-dq; +% % [~,dhp]=NonLinCon_ClosedLoop_Num(Human_model,solid_path1,solid_path2,num_solid,num_markers,qp,k); +% % [~,dhm]=NonLinCon_ClosedLoop_Num(Human_model,solid_path1,solid_path2,num_solid,num_markers,qm,k); +% % K(:,qchoix)=(dhp-dhm)/(2*dq); +% Ktest(:,qchoix) = ConstraintProjection(Human_model,solid_path1,solid_path2,num_solid,num_markers,q,k,qchoix,"one"); +% end + +K = sym(zeros(1,length(q))); + for qchoix=1:length(q) -% qp=q; -% qm=q; -% qp(qchoix)=qp(qchoix)+dq; -% qm(qchoix)=qm(qchoix)-dq; -% [~,dhp]=NonLinCon_ClosedLoop_Num(BiomechanicalModel.OsteoArticularModel,solid_path1,solid_path2,num_solid,num_markers,qp,k); -% [~,dhm]=NonLinCon_ClosedLoop_Num(BiomechanicalModel.OsteoArticularModel,solid_path1,solid_path2,num_solid,num_markers,qm,k); -% K(:,qchoix)=(dhp-dhm)/(2*dq); - K(:,qchoix) = ConstraintProjection(BiomechanicalModel.OsteoArticularModel,solid_path1,solid_path2,num_solid,num_markers,q,k,qchoix,"one"); + Human_model(qchoix).q = q(qchoix); +end + +for pp=1:numel(num_solid) + + path1 = solid_path1{pp}; + path2 = solid_path2{pp}; + ind1 = find(path1==qchoix); + + + [Human_model.R] = deal(zeros(3)); + [Human_model.Rproj] = deal(zeros(3)); + [Human_model.p] = deal([0 0 0]'); + [Human_model.pproj] = deal([0 0 0]'); + + + if Human_model(path1(1)).joint == 1 % liaison pivot + Human_model(path1(1)).R = Rodrigues(Human_model(path1(1)).a,q(path1(1)))*Rodrigues(Human_model(path1(1)).u,Human_model(path1(1)).theta); % orientation du rep�re + end + if Human_model(path1(1)).joint == 2 % liaison glissi�re + Human_model(path1(1)).p=Human_model(path1(1)).b + angle*Human_model(path1(1)).a; + end + + Human_model=ForwardPositionsandProj(Human_model,2,path1); + + Human_model3 = Human_model; + [Human_model3.R] = deal(zeros(3)); + [Human_model3.Rproj] = deal(zeros(3)); + [Human_model3.p] = deal([0 0 0]'); + [Human_model3.pproj] = deal([0 0 0]'); + + if Human_model3(path2(1)).joint == 1 % liaison pivot + Human_model3(path2(1)).R = Rodrigues(Human_model3(path2(1)).a,q(path2(1)))*Rodrigues(Human_model3(path1(1)).u,Human_model(path1(1)).theta); % orientation du rep�re + end + if Human_model3(path2(1)).joint == 2 % liaison glissi�re + Human_model3(path2(1)).p=Human_model3(path2(1)).b + angle*Human_model3(path2(1)).a; + end + + + Human_model3=ForwardPositionsandProj(Human_model3,2,path2); + + s = Human_model(num_solid(pp)).c + Human_model(num_solid(pp)).anat_position{num_markers(pp),2}; % position with respects to the position of the mother solid joint of the closed loop + + for kk=1:length(path1) + if Human_model(path1(kk)).joint ==1 + K((1:3)+6*(pp-1),path1(kk)) = diag(Human_model(path1(kk)).Rproj*Human_model3(path2(end)).R'); + K((4:6)+6*(pp-1),path1(kk)) = ~isempty(ind1)*Human_model(path1(kk)).Rproj*s +... + (-1)^(isempty(ind1))*(isempty(ind1))*Human_model(path1(kk)).pproj; + else + K((4:6)+6*(pp-1),path1(kk)) = (-1)^(isempty(ind1))*Human_model(path1(kk)).R*Human_model(path1(kk)).a; + end + end + + + for kk=1:length(path2) + if Human_model(path2(kk)).joint ==1 + K((1:3)+6*(pp-1),path2(kk)) = diag(Human_model3(path2(kk)).Rproj*Human_model(path1(end)).R'); + K((4:6)+6*(pp-1),path2(kk)) = isempty(ind1)*Human_model3(path2(kk)).Rproj*s+... + (-1)^(~isempty(ind1))*(~isempty(ind1))*Human_model3(path2(kk)).pproj; + else + K((4:6)+6*(pp-1),path2(kk)) = (-1)^(~isempty(ind1))*Human_model3(path2(kk)).R*Human_model3(path2(kk)).a; + end + end + end if ~isempty(dependancies) for pp=1:size(dependancies,2) K(size(K,1)+1,dependancies(pp).solid) = -1; - for j=1:size(dependancies(pp).Joint,1) - qchoix=dependancies(pp).Joint(j); - qp=q; - qm=q; - qp(qchoix)=qp(qchoix)+dq; - qm(qchoix)=qm(qchoix)-dq; - f=dependancies(pp).q; - if size(dependancies(pp).Joint,1)==1 - dhp=f(qp(dependancies(pp).Joint(1))); - dhm=f(qm(dependancies(pp).Joint(1))); + + df = dependancies(pp).dq; + + if size(dependancies(pp).Joint,1)==1 + K(size(K,1),dependancies(pp).Joint)= df(q(dependancies(pp).Joint)); else if size(dependancies(pp).Joint,1)==2 - dhp=f(qp(dependancies(pp).Joint(1)),qp(dependancies(pp).Joint(2))); - dhm=f(qm(dependancies(pp).Joint(1)),qm(dependancies(pp).Joint(2))); + K(size(K,1),dependancies(pp).Joint)= df(q(dependancies(pp).Joint(1)),q(dependancies(pp).Joint(2))); end end - K(size(K,1),qchoix)=(dhp-dhm)/(2*dq); - - end + end end diff --git a/Functions/Kinematics/ConstraintsJacobianDerivative.m b/Functions/Kinematics/ConstraintsJacobianDerivative.m new file mode 100644 index 00000000..b8c952bc --- /dev/null +++ b/Functions/Kinematics/ConstraintsJacobianDerivative.m @@ -0,0 +1,72 @@ +function Kdev=ConstraintsJacobianDerivative(q,dq) +% Return the constraint matrix K, which is the jacobian of the constraints +% by q + +% INPUT +% - Human_model: osteo-articular model (see the Documentation for the +% structure) +% - q: vector of joint coordinates at a given instant +% - solid_path1 : vector of one of the two paths to close the loop +% - solid_path2 : vector of the other of the two paths to close the loop +% - num_solid : vector of the number of solid where the anatomical point must join the +% origin of another joint to close the loo +% - num_markers : vector of the position in the list "anat_position" that +% corresponds to the point to close the loop +% - k: vector of homothety coefficient +% - dp: differentiation step +% - dependancies: structure where are defined all kinematic dependancies + +% OUTPUT +% - K : matrix of the derivatives of the constraints by q +%________________________________________________________ +% +% Licence +% Toolbox distributed under GPL 3.0 Licence +%________________________________________________________ +% +% Authors : Antoine Muller, Charles Pontonnier, Pierre Puchaud and +% Georges Dumont +%________________________________________________________ +for qchoix=1:length(q) + qp=q; + qm=q; + qp(qchoix)=qp(qchoix)+dq; + qm(qchoix)=qm(qchoix)-dq; + Kp = Jacobian_closedloop_fullq(qp)'; + Km = Jacobian_closedloop_fullq(qm)'; + Kdev(:,:,qchoix) = (Kp - Km)/(2*dq); + +end + + +% Closed loop constraints +% for qchoix=1:length(q) +% +% +% % Kdev2(:,:,qchoix) = ConstraintProjectionDerivative(BiomechanicalModel.OsteoArticularModel,solid_path1,solid_path2,num_solid,num_markers,q,k,qchoix,"one"); +% end +% +% +% K=[]; +% +% if ~isempty(dependancies) +% for pp=1:size(dependancies,2) +% K(size(K,1)+1,dependancies(pp).solid,dependancies(pp).solid) = 0; +% +% ddf = dependancies(pp).ddq; +% +% if size(dependancies(pp).Joint,1)==1 +% K(size(K,1),dependancies(pp).Joint,dependancies(pp).Joint)= ddf(q(dependancies(pp).Joint)); +% +% else +% if size(dependancies(pp).Joint,1)==2 +% K(size(K,1),[dependancies(pp).Joint],... +% [dependancies(pp).Joint])= ddf(q(dependancies(pp).Joint(1)),q(dependancies(pp).Joint(2))); +% end +% end +% +% end +% end + + +end \ No newline at end of file diff --git a/Functions/Kinematics/CostFunctionLM.m b/Functions/Kinematics/CostFunctionLM.m index 69bf2a60..fd1ce993 100644 --- a/Functions/Kinematics/CostFunctionLM.m +++ b/Functions/Kinematics/CostFunctionLM.m @@ -1,6 +1,6 @@ -function func=CostFunctionLM(q,positions,gamma,hclosedloophandle,zeta,hbutees,weights) +function [func,J]=CostFunctionLM(q,positions,gamma,hclosedloophandle,zeta,hbutees,weights, l_inf1,l_sup1,Aeq_ik,J_marqueurs_handle) % Limit penalisation for LM algorithm -% +% % INPUT % - q: vector of joint coordinates at a given instant % - positions : vector of experimental marker positions @@ -21,19 +21,23 @@ %________________________________________________________ - % cut evaluation - [Rcut,pcut]=fcut(q); +% cut evaluation +[Rcut,pcut]=fcut(q); + +newweights= repmat(weights,3,1); + +% dx +dX = newweights(:).*(-X_markers(q,pcut,Rcut) + positions); + +constraints=[]; +for k=1:length(hclosedloophandle) + fonc = hclosedloophandle{k}; + constraints= [constraints; fonc(q)]; +end +func = [ dX ; gamma*constraints(:) ; zeta*hbutees(q)]; + +if nargout > 1 % Two output arguments + J = IK_Jacobian(q,pcut,Rcut, l_inf1,l_sup1,Aeq_ik,gamma,zeta, J_marqueurs_handle,newweights(:)); % Jacobian of the function evaluated at q +end - newweights= repmat(weights,1,3)'; - % dx - dX = newweights(:).*(-X_markers(q,pcut,Rcut) + positions); - - constraints=[]; - for k=1:length(hclosedloophandle) - fonc = hclosedloophandle{k}; - constraints = [constraints ; fonc(q)]; - end - - func = [ dX ; gamma*constraints ; zeta*hbutees(q)]; - end \ No newline at end of file diff --git a/Functions/Kinematics/CostFunctionSymbolicIK2.m b/Functions/Kinematics/CostFunctionSymbolicIK2.m index aaf58866..f51aa376 100644 --- a/Functions/Kinematics/CostFunctionSymbolicIK2.m +++ b/Functions/Kinematics/CostFunctionSymbolicIK2.m @@ -1,4 +1,4 @@ -function [error] = CostFunctionSymbolicIK2(q,positions,weights) +function [error,J] = CostFunctionSymbolicIK2(q,positions,weights,J_marqueurs_handle) % Cost function used for the inverse kinematics step using an optimization method % % INPUT @@ -18,8 +18,10 @@ [Rcut,pcut]=fcut(q); % Vectorial norm of marker distance -newweights= repmat(weights,1,3)'; +newweights= repmat(weights,3,1); a = sum(newweights(:).*(-X_markers(q,pcut,Rcut) + positions).^2); error = sum(a(~isnan(a))); +J = -2*(-X_markers(q,pcut,Rcut) + positions)'* J_marqueurs_handle(q,pcut,Rcut); + end \ No newline at end of file diff --git a/Functions/Kinematics/Data_ClosedLoop.m b/Functions/Kinematics/Data_ClosedLoop.m index 1061aba8..df3f6cfd 100644 --- a/Functions/Kinematics/Data_ClosedLoop.m +++ b/Functions/Kinematics/Data_ClosedLoop.m @@ -9,7 +9,7 @@ % - solid_path1 : vector of one of the two paths to close the loop % - solid_path2 : vector of the other of the two paths to close the loop % - num_solid : vector of the number of solid where the anatomical point must join the -% origin of another joint to close the loo +% origin of another joint to close the loop % - num_markers : vector of the position in the list "anat_position" that % corresponds to the point to close the loop diff --git a/Functions/Kinematics/ErrorMarkersIK.m b/Functions/Kinematics/ErrorMarkersIK.m index 594139d7..7e0ae135 100644 --- a/Functions/Kinematics/ErrorMarkersIK.m +++ b/Functions/Kinematics/ErrorMarkersIK.m @@ -1,6 +1,6 @@ -function [error] = ErrorMarkersIK(q,positions) +function [error] = ErrorMarkersIK(q,positions,varargin) % Computation of reconstruction error for the inverse kinematics step -% Computation of the distance between the position of each experimental +% Computation of the distance between the position of each experimental % marker and the position of the corresponded model marker % % INPUT @@ -8,7 +8,7 @@ % - positions : vector of experimental marker positions % OUTPUT % - error: distance between the position of each experimental marker and -% the position of the corresponded model marker +% the position of the corresponded model marker %________________________________________________________ % % Licence @@ -20,6 +20,13 @@ %________________________________________________________ [Rcut,pcut]=fcut(q); -error = sqrt(sum(reshape(-X_markers(q,pcut,Rcut) + positions,3,length(positions)/3).^2,1)); +if isempty(varargin) + error = sqrt(sum(reshape(-X_markers(q,pcut,Rcut) + positions,3,length(positions)/3).^2,1)); +else + weights = varargin{1}; + newweights= repmat(weights,3,1); + error = sqrt(sum(reshape(newweights(:).*(-X_markers(q,pcut,Rcut) + positions),3,length(positions)/3).^2)); + + % error = sqrt(sum(reshape(-X_markers(q,pcut,Rcut) + positions,3,length(positions)/3).^2,1)); end \ No newline at end of file diff --git a/Functions/Kinematics/ForwardKinematics_ClosedLoop.m b/Functions/Kinematics/ForwardKinematics_ClosedLoop.m index 549ba5b4..0528b683 100644 --- a/Functions/Kinematics/ForwardKinematics_ClosedLoop.m +++ b/Functions/Kinematics/ForwardKinematics_ClosedLoop.m @@ -1,4 +1,4 @@ -function [Human_model,p,R] = ForwardKinematics_ClosedLoop(Human_model,n,s,solid_path,p,R,q,k) +function [Human_model,p,R] = ForwardKinematics_ClosedLoop(Human_model,n,s,solid_path,p,R,q,k,radius) % Computation of a symbolic forward kinematics % % INPUT @@ -13,6 +13,7 @@ % - R: matrix rotatio of the closed loops % - q: vector of joint coordinates % - k: vector of homothety coefficient +% - radius : vector of ellipsoid radius % OUTPUT % - Human_model: osteo-articular model (see the Documentation for the % structure) @@ -35,51 +36,241 @@ %% incr�mentation de la position et orientation if n ~= 0 - % if n~=(numel(solid_path)) - j=solid_path(n); % num�ro du solide - i=Human_model(j).mother; % num�ro de la m�re - - if size(Human_model(j).linear_constraint) == [0 0] % si coordonn�e articulaire fonction lin�aire d'une autre coordonn�e articulaire - angle=q(j); - else - angle=Human_model(j).linear_constraint(2)*q(Human_model(j).linear_constraint(1)); % qj=alpha*q - end - - if Human_model(j).joint == 1 % liaison pivot - p=R*(k(i)*Human_model(j).b)+p; % position du rep�re - R=R*Rodrigues(Human_model(j).a,angle)*Rodrigues(Human_model(j).u,Human_model(j).theta); % orientation du rep�re - end - if Human_model(j).joint == 2 % liaison glissi�re - p=R*(k(i)*Human_model(j).b + angle*Human_model(j).a)+p; - R=R*Rodrigues(Human_model(j).u,Human_model(j).theta); - end - - % else -% if n==(numel(solid_path)) && ~isempty(Human_model(solid_path(n)).ClosedLoop) -% j=solid_path(n); % num�ro du solide -% i=Human_model(j).mother; % num�ro de la m�re -% -% if size(Human_model(j).linear_constraint) == [0 0] % si coordonn�e articulaire fonction lin�aire d'une autre coordonn�e articulaire -% angle=-q(j); -% else -% angle=-Human_model(j).linear_constraint(2)*q(Human_model(j).linear_constraint(1)); % qj=alpha*q -% end -% -% if Human_model(j).joint == 1 % liaison pivot -% p=R*(k(i)*Human_model(j).b)+p; % position du rep�re -% R=R*Rodrigues(Human_model(j).a,angle)*Rodrigues(Human_model(j).u,Human_model(j).theta); % orientation du rep�re -% end -% if Human_model(j).joint == 2 % liaison glissi�re -% p=R*(k(i)*Human_model(j).b + angle*Human_model(j).a)+p; -% R=R*Rodrigues(Human_model(j).u,Human_model(j).theta); -% end - -% end - % end + % if n~=(numel(solid_path)) + j=solid_path(n); % num�ro du solide + i=Human_model(j).mother; % num�ro de la m�re + + if size(Human_model(j).linear_constraint) == [0 0] % si coordonn�e articulaire fonction lin�aire d'une autre coordonn�e articulaire + angle=q(j); + else + angle=Human_model(j).linear_constraint(2)*q(Human_model(j).linear_constraint(1)); % qj=alpha*q + end + + %ajout parafencing pour palier a la contrainte qui ne depend pas de + %la gueule de l'ellipse je crois + + switch Human_model(j).name + case 'RScapuloThoracic_J1' + + if sum(contains({Human_model.name},'RScapuloThoracic_Jalpha')) + [~,idx_theta1] = intersect({Human_model.name},'RScapuloThoracic_J4'); + [~,idx_theta2] = intersect({Human_model.name},'RScapuloThoracic_J5bis'); + angle= -radius(1)*cos(q(idx_theta1))*cos(q(idx_theta2)); %jmettrais bien un moins ici pr correspondre au x de la + else + %theta elevation + [~,idx_theta] = intersect({Human_model.name},'RScapuloThoracic_J4'); + %phi abduction + [~,idx_phi] = intersect({Human_model.name},'RScapuloThoracic_J5'); + + + angle= -radius(1)*cos(q(idx_theta))*cos(q(idx_phi)); + end + + case 'RScapuloThoracic_J2' + if sum(contains({Human_model.name},'RScapuloThoracic_Jalpha')) + + [~,idx_theta2] = intersect({Human_model.name},'RScapuloThoracic_J5bis'); + angle= radius(2)*sin(q(idx_theta2)); + + else + + %theta elevation + [~,idx_theta] = intersect({Human_model.name},'RScapuloThoracic_J4'); + + angle= radius(2)*sin(q(idx_theta)); + + end + case 'RScapuloThoracic_J3' + + + if sum(contains({Human_model.name},'RScapuloThoracic_Jalpha')) + + [~,idx_theta1] = intersect({Human_model.name},'RScapuloThoracic_J4'); + [~,idx_theta2] = intersect({Human_model.name},'RScapuloThoracic_J5bis'); + angle= radius(3)*sin(q(idx_theta1))*cos(q(idx_theta2)); + + else + + %theta elevation + [~,idx_theta] = intersect({Human_model.name},'RScapuloThoracic_J4'); + %phi abduction + [~,idx_phi] = intersect({Human_model.name},'RScapuloThoracic_J5'); + + angle=radius(3)*cos(q(idx_theta))*sin(q(idx_phi)); + end + + case 'LScapuloThoracic_J1' + + + if sum(contains({Human_model.name},'LScapuloThoracic_Jalpha')) + [~,idx_theta1] = intersect({Human_model.name},'LScapuloThoracic_J4'); + [~,idx_theta2] = intersect({Human_model.name},'LScapuloThoracic_J5bis'); + angle= radius(4)*cos(q(idx_theta1))*cos(q(idx_theta2)); + + else + + %theta elevation + [~,idx_theta] = intersect({Human_model.name},'LScapuloThoracic_J4'); + %phi abduction + [~,idx_phi] = intersect({Human_model.name},'LScapuloThoracic_J5'); + + angle= -radius(4)*cos(q(idx_theta))*cos(q(idx_phi)); + + + + end + + case 'LScapuloThoracic_J2' + + + if sum(contains({Human_model.name},'LScapuloThoracic_Jalpha')) + + [~,idx_theta2] = intersect({Human_model.name},'LScapuloThoracic_J5bis'); + angle= radius(5)*sin(q(idx_theta2)); + + else + + %theta elevation + [~,idx_theta] = intersect({Human_model.name},'LScapuloThoracic_J4'); + + angle=radius(5)*sin(q(idx_theta)); + + end + + case 'LScapuloThoracic_J3' + + if sum(contains({Human_model.name},'LScapuloThoracic_Jalpha')) + + [~,idx_theta1] = intersect({Human_model.name},'LScapuloThoracic_J4'); + [~,idx_theta2] = intersect({Human_model.name},'LScapuloThoracic_J5bis'); + angle= -radius(6)*sin(q(idx_theta1))*cos(q(idx_theta2)); + + else + + %theta elevation + [~,idx_theta] = intersect({Human_model.name},'LScapuloThoracic_J4'); + %phi abduction + [~,idx_phi] = intersect({Human_model.name},'LScapuloThoracic_J5'); + + angle=-radius(6)*cos(q(idx_theta))*sin(q(idx_phi)); + + + end + + + case 'RScapuloThoracic_J5' + if sum(contains({Human_model.name},'RScapuloThoracic_Jalpha')) + + [~,idx_theta1] = intersect({Human_model.name},'RScapuloThoracic_J4'); + [~,idx_theta2] = intersect({Human_model.name},'RScapuloThoracic_J5bis'); + angle = atan(-(radius(1)*radius(3)*tan(q(idx_theta2))*(1 + tan(q(idx_theta1))^2))/(radius(3)*radius(2)+radius(1)*radius(2)*tan(q(idx_theta1))^2)); + + end + + case 'RScapuloThoracic_Jalpha' + [~,idx_theta1] = intersect({Human_model.name},'RScapuloThoracic_J4'); + [~,idx_theta2] = intersect({Human_model.name},'RScapuloThoracic_J5bis'); + phi= atan(-(radius(1)*radius(3)*tan(q(idx_theta2))*(1 + tan(q(idx_theta1))^2))/(radius(3)*radius(2)+radius(1)*radius(2)*tan(q(idx_theta1))^2)); + angle= atan(tan(q(idx_theta1))*(-radius(2)/radius(3)*sin(phi)/tan(q(idx_theta2))-cos(phi))); + + + case 'LScapuloThoracic_J5' + if sum(contains({Human_model.name},'LScapuloThoracic_Jalpha')) + + [~,idx_theta1] = intersect({Human_model.name},'LScapuloThoracic_J4'); + [~,idx_theta2] = intersect({Human_model.name},'LScapuloThoracic_J5bis'); + angle = atan(-(radius(4)*radius(6)*tan(q(idx_theta2))*(1 + tan(q(idx_theta1))^2))/(radius(6)*radius(5)+radius(4)*radius(5)*tan(q(idx_theta1))^2)); + + end + + case 'LScapuloThoracic_Jalpha' + + [~,idx_theta1] = intersect({Human_model.name},'LScapuloThoracic_J4'); + [~,idx_theta2] = intersect({Human_model.name},'LScapuloThoracic_J5bis'); + phi=atan(-(radius(4)*radius(6)*tan(q(idx_theta2))*(1 + tan(q(idx_theta1))^2))/(radius(6)*radius(5)+radius(4)*radius(5)*tan(q(idx_theta1))^2)); + angle= atan(tan(q(idx_theta1))*(-radius(5)/radius(6)*sin(phi)/tan(q(idx_theta2))-cos(phi))); + + + end +% %Rscapula +% if j==10 +% +% [~,idx_theta1] = intersect({Human_model.name},'RScapuloThoracic_J4'); +% [~,idx_theta2] = intersect({Human_model.name},'RScapuloThoracic_J5bis'); +% angle= -radius(1)*cos(q(idx_theta1))*cos(q(idx_theta2)); %jmettrais bien un moins ici pr correspondre au x de la +% end +% +% +% if j==11 +% +% [~,idx_theta2] = intersect({Human_model.name},'RScapuloThoracic_J5bis'); +% angle= radius(2)*sin(q(idx_theta2)); +% end +% +% if j==12 +% [~,idx_theta1] = intersect({Human_model.name},'RScapuloThoracic_J4'); +% [~,idx_theta2] = intersect({Human_model.name},'RScapuloThoracic_J5bis'); +% angle= radius(3)*sin(q(idx_theta1))*cos(q(idx_theta2)); +% end + +% %LScapula +% if j==25 +% +% [~,idx_theta1] = intersect({Human_model.name},'RScapuloThoracic_J4'); +% [~,idx_theta2] = intersect({Human_model.name},'RScapuloThoracic_J5bis'); +% angle= -radius(1)*cos(q(idx_theta1))*cos(q(idx_theta2)); %jmettrais bien un moins ici pr correspondre au x de la +% end +% +% +% if j==26 +% +% [~,idx_theta2] = intersect({Human_model.name},'RScapuloThoracic_J5bis'); +% angle= radius(2)*sin(q(idx_theta2)); +% end +% +% if j==27 +% [~,idx_theta1] = intersect({Human_model.name},'RScapuloThoracic_J4'); +% [~,idx_theta2] = intersect({Human_model.name},'RScapuloThoracic_J5bis'); +% angle= radius(3)*sin(q(idx_theta1))*cos(q(idx_theta2)); +% end + %fin ajout + + + if Human_model(j).joint == 1 % liaison pivot + p=R*(k(i)*Human_model(j).b)+p; % position du rep�re + R=R*Rodrigues(Human_model(j).a,angle)*Rodrigues(Human_model(j).u,Human_model(j).theta); % orientation du rep�re + end + if Human_model(j).joint == 2 % liaison glissi�re + p=R*(k(i)*Human_model(j).b + angle*Human_model(j).a)+p; + R=R*Rodrigues(Human_model(j).u,Human_model(j).theta); + end + + % else + % if n==(numel(solid_path)) && ~isempty(Human_model(solid_path(n)).ClosedLoop) + % j=solid_path(n); % num�ro du solide + % i=Human_model(j).mother; % num�ro de la m�re + % + % if size(Human_model(j).linear_constraint) == [0 0] % si coordonn�e articulaire fonction lin�aire d'une autre coordonn�e articulaire + % angle=-q(j); + % else + % angle=-Human_model(j).linear_constraint(2)*q(Human_model(j).linear_constraint(1)); % qj=alpha*q + % end + % + % if Human_model(j).joint == 1 % liaison pivot + % p=R*(k(i)*Human_model(j).b)+p; % position du rep�re + % R=R*Rodrigues(Human_model(j).a,angle)*Rodrigues(Human_model(j).u,Human_model(j).theta); % orientation du rep�re + % end + % if Human_model(j).joint == 2 % liaison glissi�re + % p=R*(k(i)*Human_model(j).b + angle*Human_model(j).a)+p; + % R=R*Rodrigues(Human_model(j).u,Human_model(j).theta); + % end + + % end + % end end n=n+1; -[Human_model,p,R] = ForwardKinematics_ClosedLoop(Human_model,n,s,solid_path,p,R,q,k); +[Human_model,p,R] = ForwardKinematics_ClosedLoop(Human_model,n,s,solid_path,p,R,q,k,radius); end \ No newline at end of file diff --git a/Functions/Kinematics/ForwardPositionsandProj.m b/Functions/Kinematics/ForwardPositionsandProj.m new file mode 100644 index 00000000..0cd629e4 --- /dev/null +++ b/Functions/Kinematics/ForwardPositionsandProj.m @@ -0,0 +1,61 @@ +function [Human_model] = ForwardPositionsandProj(Human_model,n,solid_path) +% Computation of spacial position and rotation for each solid starting from +% j +% +% INPUT +% - Human_model: osteo-articular model (see the Documentation for the +% structure) +% - j: current solid +% OUTPUT +% - Human_model: osteo-articular model with additional computations (see +% the Documentation for the structure) +%________________________________________________________ +% +% Licence +% Toolbox distributed under GPL 3.0 Licence +%________________________________________________________ +% +% Authors : Antoine Muller, Charles Pontonnier, Pierre Puchaud and +% Georges Dumont +%________________________________________________________ + +if n==(numel(solid_path)+1) % quand on arrive au bout de la cha�ne + return; +end + +%% Position vector and Rotation Matrix computation +if n~=0 + j=solid_path(n); % num�ro du solide + i=Human_model(j).mother; + + % Pin joint + if Human_model(j).joint == 1 + Human_model(j).p=Human_model(i).R*Human_model(j).b+Human_model(i).p; + Human_model(j).R=Human_model(i).R*Rodrigues(Human_model(j).a,Human_model(j).q)*Rodrigues(Human_model(j).u,Human_model(j).theta); + + for kk=1:numel(solid_path) + Human_model(solid_path(kk)).Rproj = Human_model(solid_path(kk)).Rproj*Rodrigues(Human_model(j).a,Human_model(j).q)*Rodrigues(Human_model(j).u,Human_model(j).theta); + Human_model(solid_path(kk)).pproj = Human_model(solid_path(kk)).pproj+Human_model(solid_path(kk)).Rproj *Human_model(j).b; + end + + Human_model(j).Rproj=Human_model(j).R*wedge(Human_model(j).a); + end + + % Slide joint + if Human_model(j).joint == 2 + Human_model(j).p=Human_model(i).R*Human_model(j).b+Human_model(i).R*Human_model(j).a*Human_model(j).q+Human_model(i).p; + Human_model(j).R=Human_model(i).R*Rodrigues(Human_model(j).u,Human_model(j).theta); + + for kk=1:numel(solid_path) + Human_model(solid_path(kk)).Rproj =Human_model(solid_path(kk)).Rproj*Rodrigues(Human_model(j).u,Human_model(j).theta); + Human_model(solid_path(kk)).pproj = Human_model(solid_path(kk)).pproj+Human_model(solid_path(kk)).Rproj *(Human_model(j).b+Human_model(j).a*Human_model(j).q); + end + + end +end + +n = n+1; + [Human_model] = ForwardPositionsandProj(Human_model,n,solid_path); + +end + diff --git a/Functions/Kinematics/IK_Jacobian.m b/Functions/Kinematics/IK_Jacobian.m new file mode 100644 index 00000000..9c2bfce4 --- /dev/null +++ b/Functions/Kinematics/IK_Jacobian.m @@ -0,0 +1,22 @@ +function J = IK_Jacobian(q,pcut,Rcut,l_inf1,l_sup1,Aeq_ik,gamma,zeta,J_marqueurs_handle,newweights) + +J_marqueurs = J_marqueurs_handle(q,pcut,Rcut); + +J_closedloop = Jacobian_closedloop(q,pcut,Rcut); + +J_Aek = Aeq_ik; + +idxsup = q>l_sup1; +idxinf = q1 +if length(varargin)>=2 Scapulalocator = varargin{2}; if Scapulalocator.active for idx=1:2 - if ~isempty(find(strcmp(Scapulalocator.side,Side2{idx}),1)) - s=[s;{'SCLL' ['ScapLoc_AA_',Side2{idx}] {'On';'On';'On'};... - 'SCLM' ['ScapLoc_TS_',Side2{idx}] {'On';'On';'On'};... - 'SCLB' ['ScapLoc_AI_',Side2{idx}] {'On';'On';'On'};... + temp = strfind(Scapulalocator.side,Side2{idx}); + if ~isempty(temp{:}) + s=[s;{['SCAPLOCLM',Side1{idx}] ['ScapLoc_AA_',Side2{idx}] {'On';'On';'On'};... + ['SCAPLOCMM',Side1{idx}] ['ScapLoc_TS_',Side2{idx}] {'On';'On';'On'};... + ['SCAPLOCB',Side1{idx}] ['ScapLoc_AI_',Side2{idx}] {'On';'On';'On'};... }]; end end @@ -62,6 +64,7 @@ end + Markers=struct('name',{s{:,1}}','anat_position',{s{:,2}}','calib_dir',{s{:,3}}'); %#ok end \ No newline at end of file diff --git a/Functions/Models/Markers/Marker_set2.m b/Functions/Models/Markers/Marker_set2.m index d04ddad5..9f0fe3fc 100644 --- a/Functions/Models/Markers/Marker_set2.m +++ b/Functions/Models/Markers/Marker_set2.m @@ -37,10 +37,11 @@ Scapulalocator = varargin{1}; if Scapulalocator.active for idx=1:2 - if ~isempty(find(strcmp(Scapulalocator.side,Side{idx}),1)) - s=[s;{'ScapLocAA' ['ScapLoc_AA_',Side{idx}] {'On';'On';'On'};... - 'ScapLocTS' ['ScapLoc_TS_',Side{idx}] {'On';'On';'On'};... - 'ScapLocAI' ['ScapLoc_AI_',Side{idx}] {'On';'On';'On'};... + temp = strfind(Scapulalocator.side,Side{idx}); + if ~isempty(temp{:}) + s=[s;{['ScapLocAA_',Side{idx}] ['ScapLoc_AA_',Side{idx}] {'On';'On';'On'};... + ['ScapLocTS_',Side{idx}] ['ScapLoc_TS_',Side{idx}] {'On';'On';'On'};... + ['ScapLocAI_',Side{idx}] ['ScapLoc_AI_',Side{idx}] {'On';'On';'On'};... }]; end end diff --git a/Functions/Models/Osteoarticular/Arm/ModelParts/Upperarm.m b/Functions/Models/Osteoarticular/Arm/ModelParts/Upperarm.m index 608dea22..94d09b97 100644 --- a/Functions/Models/Osteoarticular/Arm/ModelParts/Upperarm.m +++ b/Functions/Models/Osteoarticular/Arm/ModelParts/Upperarm.m @@ -24,7 +24,8 @@ % Authors : Antoine Muller, Charles Pontonnier, Pierre Puchaud and % Georges Dumont %________________________________________________________ -list_solid={'Glenohumeral_J1' 'Glenohumeral_J2' 'Glenohumeral_J3' 'Humerus'}; +% list_solid={'Glenohumeral_J1' 'Glenohumeral_J2' 'Glenohumeral_J3' 'Humerus'}; +list_solid={'Glenohumeral_J1' 'Glenohumeral_J2' 'Humerus'}; %% Choose right or left arm if Signe == 'R' @@ -97,7 +98,7 @@ Humerus_Biceps_via7 = (osim2antoine.*[0.0228 -0.1754 -0.0063])*Mirror; %in local frame OSIMarm26 %Humerus_ECRL = (k*[-0.005 -0.260 -0.002])*Mirror; %in local frame gh Murray2001 -% Humerus_Brachialis = (k*[0.008 -0.184 -0.013])*Mirror; %in local frame gh Murray2001 +%Humerus_Brachialis = (k*[0.008 -0.184 -0.013])*Mirror; %in local frame gh Murray2001 %Humerus_Brachialis = (k*[0.0068 -0.1739 -0.0036])*Mirror; %in local frame OSIMarm26 %Humerus_PronatorTeres = (k*[0.003 -0.270 -0.051])*Mirror; %in local frame gh Murray2001 @@ -311,10 +312,17 @@ OsteoArticularModel(incr_solid).sister=0; % sister : defined as an input of the function OsteoArticularModel(incr_solid).child=s_Glenohumeral_J2; OsteoArticularModel(incr_solid).mother=s_mother; % mother : defined as an input of the function -OsteoArticularModel(incr_solid).a=[0 1 0]'; % rotation /x +OsteoArticularModel(incr_solid).a=[1 0 0]'; % rotation /x OsteoArticularModel(incr_solid).joint=1; -OsteoArticularModel(incr_solid).limit_inf=-pi; % inferior joint biomechanical stop -OsteoArticularModel(incr_solid).limit_sup=pi; % superior joint biomechanical stop +if Signe == 'R' + OsteoArticularModel(incr_solid).limit_inf=-4*pi/5; % inferior joint biomechanical stop + OsteoArticularModel(incr_solid).limit_sup=pi/2; % superior joint biomechanical stop + OsteoArticularModel(incr_solid).FunctionalAngle='Right GH Abduction(-)/Adduction(+)'; +else + OsteoArticularModel(incr_solid).limit_inf=-pi/2; % inferior joint biomechanical stop + OsteoArticularModel(incr_solid).limit_sup=4*pi/5; % superior joint biomechanical stop + OsteoArticularModel(incr_solid).FunctionalAngle='Left GH Abduction(+)/Adduction(-)'; +end % superior joint biomechanical stop OsteoArticularModel(incr_solid).m=0; % reference mass OsteoArticularModel(incr_solid).b=pos_attachment_pt; % attachment point with respect to the parent's frame OsteoArticularModel(incr_solid).I=zeros(3,3); % reference inertia matrix @@ -326,7 +334,6 @@ OsteoArticularModel(incr_solid).ClosedLoop=[]; % if this solid close a closed-loop chain : {number of solid i on which is attached this solid ; attachement point (local frame of solid i} OsteoArticularModel(incr_solid).linear_constraint=[]; OsteoArticularModel(incr_solid).Visual=0; -OsteoArticularModel(incr_solid).FunctionalAngle=[FullSide 'GH plane of elevation']; % Glenohumeral_J2 % Negative GH elevation (ISB recommandations: Wu et al. 2005) num_solid=num_solid+1; % number of the solid ... @@ -334,52 +341,51 @@ eval(['incr_solid=s_' name ';']) % number of the solid in the model OsteoArticularModel(incr_solid).name=[Signe name]; OsteoArticularModel(incr_solid).sister=0; -OsteoArticularModel(incr_solid).child=s_Glenohumeral_J3; -OsteoArticularModel(incr_solid).mother=s_Glenohumeral_J1; -OsteoArticularModel(incr_solid).a=[1 0 0]'; -OsteoArticularModel(incr_solid).joint=1; -if Signe == 'R' - OsteoArticularModel(incr_solid).limit_inf=-4*pi/5; % inferior joint biomechanical stop - OsteoArticularModel(incr_solid).limit_sup=pi/4; % superior joint biomechanical stop - OsteoArticularModel(incr_solid).FunctionalAngle='Right GH Elevation(-)/Depression(+)'; -else - OsteoArticularModel(incr_solid).limit_inf=-pi/4; % inferior joint biomechanical stop - OsteoArticularModel(incr_solid).limit_sup=4*pi/5; % superior joint biomechanical stop - OsteoArticularModel(incr_solid).FunctionalAngle='Left GH Elevation(+)/Depression(-)'; -end -OsteoArticularModel(incr_solid).m=0; -OsteoArticularModel(incr_solid).b=[0 0 0]'; -OsteoArticularModel(incr_solid).I=zeros(3,3); -OsteoArticularModel(incr_solid).c=[0 0 0]'; -OsteoArticularModel(incr_solid).Visual=0; - -% Glenohumeral_J3 % Negative GH elevation (ISB recommandations: Wu et al. 2005) -num_solid=num_solid+1; % number of the solid ... -name=list_solid{num_solid}; % name of the solid -eval(['incr_solid=s_' name ';']) % number of the solid in the model -OsteoArticularModel(incr_solid).name=[Signe name]; -OsteoArticularModel(incr_solid).sister=0; +% OsteoArticularModel(incr_solid).child=s_Glenohumeral_J3; OsteoArticularModel(incr_solid).child=s_Humerus; -OsteoArticularModel(incr_solid).mother=s_Glenohumeral_J2; -OsteoArticularModel(incr_solid).a=[0 1 0]'; +OsteoArticularModel(incr_solid).mother=s_Glenohumeral_J1; +OsteoArticularModel(incr_solid).a=[0 0 1]'; OsteoArticularModel(incr_solid).joint=1; -OsteoArticularModel(incr_solid).limit_inf=-pi; % inferior joint biomechanical stop +OsteoArticularModel(incr_solid).limit_inf=-pi/2; OsteoArticularModel(incr_solid).limit_sup=pi; % superior joint biomechanical stop -OsteoArticularModel(incr_solid).FunctionalAngle='Negative GH plane of elevation'; -OsteoArticularModel(incr_solid).comment='Negative GH plane of elevation'; +OsteoArticularModel(incr_solid).FunctionalAngle='GH Plane of Elevation(+)/Depression(-)'; OsteoArticularModel(incr_solid).m=0; OsteoArticularModel(incr_solid).b=[0 0 0]'; OsteoArticularModel(incr_solid).I=zeros(3,3); OsteoArticularModel(incr_solid).c=[0 0 0]'; OsteoArticularModel(incr_solid).Visual=0; -% Dependancy -OsteoArticularModel(incr_solid).kinematic_dependancy.active=1; -OsteoArticularModel(incr_solid).kinematic_dependancy.Joint=incr_solid-2; % Thoracicellips -% Kinematic dependancy function -syms theta_plane real -plane_elev = -theta_plane; -f_plane_elev = matlabFunction(plane_elev,'vars',{theta_plane}); -OsteoArticularModel(incr_solid).kinematic_dependancy.q=f_plane_elev; + +% % Glenohumeral_J3 % Negative GH elevation (ISB recommandations: Wu et al. 2005) +% num_solid=num_solid+1; % number of the solid ... +% name=list_solid{num_solid}; % name of the solid +% eval(['incr_solid=s_' name ';']) % number of the solid in the model +% OsteoArticularModel(incr_solid).name=[Signe name]; +% OsteoArticularModel(incr_solid).sister=0; +% OsteoArticularModel(incr_solid).child=s_Humerus; +% OsteoArticularModel(incr_solid).mother=s_Glenohumeral_J2; +% OsteoArticularModel(incr_solid).a=[0 1 0]'; +% OsteoArticularModel(incr_solid).joint=1; +% OsteoArticularModel(incr_solid).limit_inf=-pi; % inferior joint biomechanical stop +% OsteoArticularModel(incr_solid).limit_sup=pi; % superior joint biomechanical stop +% OsteoArticularModel(incr_solid).FunctionalAngle='Negative GH plane of elevation'; +% OsteoArticularModel(incr_solid).comment='Negative GH plane of elevation'; +% OsteoArticularModel(incr_solid).m=0; +% OsteoArticularModel(incr_solid).b=[0 0 0]'; +% OsteoArticularModel(incr_solid).I=zeros(3,3); +% OsteoArticularModel(incr_solid).c=[0 0 0]'; +% OsteoArticularModel(incr_solid).Visual=0; +% % Dependancy +% OsteoArticularModel(incr_solid).kinematic_dependancy.active=1; +% OsteoArticularModel(incr_solid).kinematic_dependancy.Joint=incr_solid-2; % Thoracicellips +% % Kinematic dependancy function +% syms theta_plane real +% plane_elev = -theta_plane; +% f_plane_elev = matlabFunction(plane_elev,'vars',{theta_plane}); +% df_plane_elev = matlabFunction(jacobian(plane_elev,theta_plane),'vars',{theta_plane}); +% ddf_plane_elev = matlabFunction(jacobian(jacobian(plane_elev,theta_plane),theta_plane),'vars',{theta_plane}); +% OsteoArticularModel(incr_solid).kinematic_dependancy.q=f_plane_elev; +% OsteoArticularModel(incr_solid).kinematic_dependancy.dq=df_plane_elev; +% OsteoArticularModel(incr_solid).kinematic_dependancy.ddq=ddf_plane_elev; % Humerus % GH axial rotation (ISB recommandations: Wu et al. 2005) num_solid=num_solid+1; % number of the solid ... @@ -388,16 +394,17 @@ OsteoArticularModel(incr_solid).name=[Signe name]; OsteoArticularModel(incr_solid).sister=0; OsteoArticularModel(incr_solid).child=0; -OsteoArticularModel(incr_solid).mother=s_Glenohumeral_J3; +% OsteoArticularModel(incr_solid).mother=s_Glenohumeral_J3; +OsteoArticularModel(incr_solid).mother=s_Glenohumeral_J2; OsteoArticularModel(incr_solid).a=[0 1 0]'; OsteoArticularModel(incr_solid).joint=1; if Signe == 'R' - OsteoArticularModel(incr_solid).limit_inf=-pi/2; - OsteoArticularModel(incr_solid).limit_sup=pi/6; % superior joint biomechanical stop + OsteoArticularModel(incr_solid).limit_inf=-pi; + OsteoArticularModel(incr_solid).limit_sup=pi; % superior joint biomechanical stop OsteoArticularModel(incr_solid).FunctionalAngle='Right GH Axial rotation Internal(+)/External(-)'; else - OsteoArticularModel(incr_solid).limit_inf=-pi/6; - OsteoArticularModel(incr_solid).limit_sup=pi/2; % superior joint biomechanical stop + OsteoArticularModel(incr_solid).limit_inf=-pi; + OsteoArticularModel(incr_solid).limit_sup=pi; % superior joint biomechanical stop OsteoArticularModel(incr_solid).FunctionalAngle='Left GH Axial rotation Internal(-)/External(+)'; end OsteoArticularModel(incr_solid).m=Mass.UpperArm_Mass; diff --git a/Functions/Models/Osteoarticular/UpperTrunk/ModelParts/Scapula_Shoulder.m b/Functions/Models/Osteoarticular/UpperTrunk/ModelParts/Scapula_Shoulder.m index 5309164c..fd4eab64 100644 --- a/Functions/Models/Osteoarticular/UpperTrunk/ModelParts/Scapula_Shoulder.m +++ b/Functions/Models/Osteoarticular/UpperTrunk/ModelParts/Scapula_Shoulder.m @@ -231,11 +231,12 @@ Scapulalocator = varargin{1}; Scapulalocator = Scapulalocator{1, 1}; if Scapulalocator.active - if ~isempty(find(strcmp(Scapulalocator.side,Side),1)) + temp = strfind(Scapulalocator.side,Side); + if ~isempty(temp{:}) vec_1 = Scapula_locator_AA - Scapula_locator_AI; vec_2 = Scapula_locator_TS - Scapula_locator_AI; normal= Sign*cross(vec_1,vec_2)/norm(cross(vec_1,vec_2)); - ind = find(strcmp(Scapulalocator.side,Side),1); + ind = 1; Scapula_locator_AA = Scapula_locator_AA + Scapulalocator.height(ind)*normal*1e-2; Scapula_locator_AI = Scapula_locator_AI + Scapulalocator.height(ind)*normal*1e-2; Scapula_locator_TS = Scapula_locator_TS + Scapulalocator.height(ind)*normal*1e-2; @@ -265,11 +266,6 @@ %% Scapulo-thoracic joint syms theta phi real% latitude longitude -% x = -Thorax_Rx*cos(theta)*cos(phi); -% -% y = -Thorax_Ry*sin(theta); -% -% z = Thorax_Rz*cos(theta)*sin(phi); x = -Thorax_Rx*cos(theta)*cos(phi); y = Thorax_Ry*sin(theta); @@ -304,7 +300,11 @@ Human_model(incr_solid).kinematic_dependancy.Joint=[incr_solid+3; incr_solid+4]; % Thoracicellips % Kinematic dependancy function f_tx = matlabFunction(x,'vars',{theta,phi}); +df_tx = matlabFunction(jacobian(x,[theta,phi]),'vars',{theta,phi}); +ddf_tx = matlabFunction(jacobian(jacobian(x,[theta,phi]),[theta,phi]),'vars',{theta,phi}); Human_model(incr_solid).kinematic_dependancy.q=f_tx; +Human_model(incr_solid).kinematic_dependancy.dq=df_tx; +Human_model(incr_solid).kinematic_dependancy.ddq=ddf_tx; Human_model(incr_solid).comment=[FullSide 'Scapulothoracic x displacement']; Human_model(incr_solid).FunctionalAngle=[FullSide 'Scapulothoracic x displacement']; @@ -338,7 +338,11 @@ Human_model(incr_solid).kinematic_dependancy.Joint=incr_solid+2; % Thoracicellips % Kinematic dependancy function f_ty = matlabFunction(y,'vars',{theta}); +d_fty = matlabFunction(jacobian(y,[theta]),'vars',{theta}); +dd_fty = matlabFunction(jacobian(jacobian(y,[theta]),[theta]),'vars',{theta}); Human_model(incr_solid).kinematic_dependancy.q=f_ty; +Human_model(incr_solid).kinematic_dependancy.dq=d_fty; +Human_model(incr_solid).kinematic_dependancy.ddq=dd_fty; % ScapuloThoracic_J3 num_solid=num_solid+1; % solid number @@ -370,7 +374,11 @@ Human_model(incr_solid).kinematic_dependancy.Joint=[incr_solid+1; incr_solid+2]; % Thoracicellips % Kinematic dependancy function f_tz = matlabFunction(z,'vars',{theta,phi}); +df_tz = matlabFunction(jacobian(z,[theta,phi]),'vars',{theta,phi}); +ddf_tz = matlabFunction(jacobian(jacobian(z,[theta,phi]),[theta,phi]),'vars',{theta,phi}); Human_model(incr_solid).kinematic_dependancy.q=f_tz; +Human_model(incr_solid).kinematic_dependancy.dq=df_tz; +Human_model(incr_solid).kinematic_dependancy.ddq=ddf_tz; % ScapuloThoracic_J4 num_solid=num_solid+1; % solid number diff --git a/Functions/Models/Osteoarticular/UpperTrunk/ModelParts/Scapula_Shoulder_Tangent.m b/Functions/Models/Osteoarticular/UpperTrunk/ModelParts/Scapula_Shoulder_Tangent.m index 087ccfb4..bc6c87b6 100644 --- a/Functions/Models/Osteoarticular/UpperTrunk/ModelParts/Scapula_Shoulder_Tangent.m +++ b/Functions/Models/Osteoarticular/UpperTrunk/ModelParts/Scapula_Shoulder_Tangent.m @@ -23,7 +23,7 @@ %% Solid list -list_solid={'ScapuloThoracic_J0' 'ScapuloThoracic_J1' 'ScapuloThoracic_J2' 'ScapuloThoracic_J3' 'ScapuloThoracic_J4' 'ScapuloThoracic_J5' 'ScapuloThoracic_Jalpha' 'ScapuloThoracic_J6' 'Scapula'}; +list_solid={'ScapuloThoracic_J1' 'ScapuloThoracic_J2' 'ScapuloThoracic_J3' 'ScapuloThoracic_J4' 'ScapuloThoracic_J5' 'ScapuloThoracic_J5bis' 'ScapuloThoracic_Jalpha' 'ScapuloThoracic_J6' 'Scapula'}; %% Choix jambe droite ou gauche if Side == 'R' @@ -31,13 +31,11 @@ Sign=1; Cote='D'; FullSide='Right'; -else - if Side == 'L' - Mirror=[1 0 0; 0 1 0; 0 0 -1]; - Sign=-1; - Cote='G'; - FullSide='Left'; - end +elseif Side == 'L' + Mirror=[1 0 0; 0 1 0; 0 0 -1]; + Sign=-1; + Cote='G'; + FullSide='Left'; end %% Solid numbering incremation @@ -80,28 +78,30 @@ % ------------------------- Thorax ---------------------------------------- % scaling coef based on shoulder width -CoM_Thorax = k*[0.060 0.303 0]; -Thorax_T12L1JointNode = k*[0.022 0.154 0] - CoM_Thorax; -Thorax_ShoulderRightNode = k*[-0.0408 0.1099 0.1929]-Thorax_T12L1JointNode; -Thorax_osim2antoine = [k k Thorax_ShoulderRightNode(3)/0.17]; +% CoM_Thorax = k*[0.060 0.303 0]; +% Thorax_T12L1JointNode = k*[0.022 0.154 0] - CoM_Thorax; +% Thorax_ShoulderRightNode = k*[-0.0408 0.1099 0.1929]-Thorax_T12L1JointNode; +% Thorax_osim2antoine = [k k Thorax_ShoulderRightNode(3)/0.17]; +Thorax_osim2antoine = [k k k]; % ------------------------ Scapula ---------------------------------------- % Centre of mass location in OpenSim frame Scapula_CoM = Thorax_osim2antoine.*Mirror*[-0.054694 -0.035032 -0.043734]'; % Landmarks location in CusToM frame -Scapula_ghJointNode = Thorax_osim2antoine.*Mirror*[-0.00955; -0.034; 0.009] - Scapula_CoM; -Scapula_stJointNode = Thorax_osim2antoine.*Mirror*[-0.05982; -0.03904; -0.056] - Scapula_CoM; -Scapula_acJointNode = Thorax_osim2antoine.*Mirror*[-0.01357; 0.00011; -0.01523] - Scapula_CoM; -Scapula_acromion = Thorax_osim2antoine.*Mirror*[-0.0142761 0.0131922 -0.00563961]' - Scapula_CoM; -Scapula_cluster_med = Thorax_osim2antoine.*Mirror*[-0.0860033 0.0298369 -0.00786593]' - Scapula_CoM; -Scapula_cluster_lat = Thorax_osim2antoine.*Mirror*[-0.0956621 0.0398035 -0.0552027]' - Scapula_CoM; -Scapula_cluster_mid = Thorax_osim2antoine.*Mirror*[-0.119492 0.0147336 -0.0385808]' - Scapula_CoM; -Scapula_locator_AI = Thorax_osim2antoine.*Mirror*[-0.109351 -0.1132 -0.0903848]' - Scapula_CoM; -Scapula_locator_TS = Thorax_osim2antoine.*Mirror*[-0.0874576 -0.0120416 -0.098654]' - Scapula_CoM; -Scapula_locator_AA = Thorax_osim2antoine.*Mirror*[-0.0393614 -0.00318483 0.0080672]' - Scapula_CoM; -Thorax_Rx = Thorax_osim2antoine(1)*0.07; -Thorax_Ry = Thorax_osim2antoine(2)*0.15; -Thorax_Rz = Thorax_osim2antoine(3)*0.07; +Scapula_ghJointNode = Thorax_osim2antoine.*Mirror*[-0.00955; -0.034; 0.009] - Scapula_CoM; +Scapula_stJointNode = Thorax_osim2antoine.*Mirror*[-0.05982; -0.03904; -0.056] - Scapula_CoM; +Scapula_acJointNode = Thorax_osim2antoine.*Mirror*[-0.01357; 0.00011; -0.01523] - Scapula_CoM; +Scapula_acromion_plane = Thorax_osim2antoine.*Mirror*[-0.0142761 0.0131922 -0.00563961]' - Scapula_CoM; +Scapula_acromion = Thorax_osim2antoine.*Mirror*[-0.0142761 0.0131922 -0.00563961]' - Scapula_CoM; +Scapula_cluster_lat = Thorax_osim2antoine.*Mirror*[-0.0860033 0.0298369 -0.00786593]' - Scapula_CoM; +Scapula_cluster_med = Thorax_osim2antoine.*Mirror*[-0.0956621 0.0398035 -0.0552027]' - Scapula_CoM; +Scapula_cluster_mid = Thorax_osim2antoine.*Mirror*[-0.119492 0.0147336 -0.0385808]' - Scapula_CoM; +Scapula_locator_AI = Thorax_osim2antoine.*Mirror*[-0.109351 -0.1132 -0.0903848]' - Scapula_CoM; +Scapula_locator_TS = Thorax_osim2antoine.*Mirror*[-0.0874576 -0.0120416 -0.098654]' - Scapula_CoM; +Scapula_locator_AA = Thorax_osim2antoine.*Mirror*[-0.0393614 -0.00318483 0.0080672]' - Scapula_CoM; +Thorax_Rx = Thorax_osim2antoine(1)*0.07; +Thorax_Ry = Thorax_osim2antoine(2)*0.15; +Thorax_Rz = Thorax_osim2antoine(3)*0.07; %% Definition of anatomical landmarThorax_osim2antoine.s (with respect to the center of mass of the solid) @@ -110,7 +110,8 @@ [Side 'Scapula_AcromioClavicularJointNode'], Scapula_acJointNode;... ['Thorax_Shoulder' FullSide 'Node'], Scapula_ghJointNode;... % MarThorax_osim2antoine.ers - [Side 'SHO'], Scapula_acromion;... + [Side 'SHO'], Scapula_acromion_plane;... + [Side 'Acromion summit'], Scapula_acromion;... ['MTAC' Cote 'M'], Scapula_cluster_med;... ['MTAC' Cote 'B'], Scapula_cluster_mid;... ['MTAC' Cote 'L'], Scapula_cluster_lat;... @@ -166,7 +167,59 @@ [Side 'Scapula_r_PECM1'],Thorax_osim2antoine.*Mirror*([0.01087460399498133 -0.035041350780721695 -0.022940819603399599]')-Scapula_CoM;... + % Via points from moment arm optimization + + [Side 'Scapula_Rhomboideus_I_VP2'], (k*Mirror*[0.0322 ; -0.0320 ; 0.0642] - Scapula_CoM) ;... + + [Side 'Scapula_DeltoideusScapula_P_VP1'], (k*Mirror*[-0.0412 ; 0.0083 ; 0.0617] - Scapula_CoM) ;... + + [Side 'Scapula_DeltoideusScapula_M_VP1'], (k*Mirror*[0.0563 ; 0.0306 ; 0.0823] - Scapula_CoM) ;... + + [Side 'Scapula_TrapeziusScapula_M_VP2'], (k*Mirror*[-0.0104 ; 0.0202 ; 0.0040] - Scapula_CoM) ;... + + [Side 'Scapula_TrapeziusScapula_S_VP2'], (k*Mirror*[0.0457 ; 0.2980 ; 0.0372] - Scapula_CoM) ;... + + [Side 'Scapula_TrapeziusScapula_I_VP2'], (k*Mirror*[-0.0125 ; 0.0677 ; 0.0321] - Scapula_CoM) ;... + + [Side 'Scapula_Supraspinatus_P_VP1'], (k*Mirror*[-0.0042 ; 0.0406 ; 0.0022] - Scapula_CoM) ;... + + [Side 'Scapula_Supraspinatus_A_VP1'], (k*Mirror*[-0.0063 ; 0.0640 ; -0.0577] - Scapula_CoM) ;... + + [Side 'Scapula_Infraspinatus_I_VP1'], (k*Mirror*[-0.0649 ; -0.0316 ; -0.0088] - Scapula_CoM) ;... + + [Side 'Scapula_Infraspinatus_S_VP1'], (k*Mirror*[-0.0649 ; 0.0323 ; -0.0126] - Scapula_CoM) ;... + + [Side 'Scapula_Coracobrachialis_VP1'], (k*Mirror*[0.0692 ; -0.0039 ; 0.0258] - Scapula_CoM) ;... + + [Side 'Scapula_TeresMajor_VP1'], (k*Mirror*[-0.0495 ; -0.0797 ; 0.0017] - Scapula_CoM) ;... + [Side 'Scapula_TeresMinor_VP1'], (k*Mirror*[-0.0298 ; -0.0299 ; 0.0097] - Scapula_CoM) ;... + + [Side 'Scapula_Subscapularis_I_VP1'], (k*Mirror*[-0.0070 ; -0.0503 ; -0.0016] - Scapula_CoM) ;... + + [Side 'Scapula_Subscapularis_M_VP1'], (k*Mirror*[-0.0286 ; -0.0815 ; -0.0575] - Scapula_CoM) ;... + + [Side 'Scapula_Subscapularis_S_VP1'], (k*Mirror*[0.0272 ; -0.0259 ; 0.0045] - Scapula_CoM) ;... + + [Side 'Scapula_SerratusAnterior_M_VP2'], (k*Mirror*[0.0665 ; 0.1031 ; 0.0088] - Scapula_CoM) ;... + + [Side 'Scapula_SerratusAnterior_I_VP2'], (k*Mirror*[0.0751 ; 0.1196 ; -0.0577] - Scapula_CoM) ;... + + [Side 'Scapula_PectoralisMinor_VP2'], (k*Mirror*[-0.0010 ; 0.0357 ; -0.0004] - Scapula_CoM) ;... + + [Side 'Scapula_PectoralisMajorThorax_I_VP2'], (k*Mirror*[-0.0111 ; 0.0612 ; -0.0215]- Scapula_CoM) ;... + [Side 'Scapula_PectoralisMajorThorax_I_VP3'], (k*Mirror*[0.0528 ; -0.0065 ; 0.0618] - Scapula_CoM) ;... + + [Side 'Scapula_PectoralisMajorThorax_M_VP2'], (k*Mirror*[0.0007 ; 0.0199 ; -0.0299] - Scapula_CoM) ;... + [Side 'Scapula_PectoralisMajorThorax_M_VP3'], (k*Mirror*[0.0543 ; -0.0166 ; 0.0529] - Scapula_CoM) ;... + + [Side 'Scapula_LatissimusDorsi_S_VP2'], (k*Mirror*[-0.0451 ; 0.0826 ; -0.0309] - Scapula_CoM) ;... + [Side 'Scapula_LatissimusDorsi_S_VP3'], (k*Mirror*[0.0551 ; 0.0065 ; 0.0579] - Scapula_CoM) ;... + + [Side 'Scapula_LatissimusDorsi_I_VP2'], (k*Mirror*[0.0719 ; -0.1432 ; 0.0671] - Scapula_CoM) ;... + [Side 'Scapula_LatissimusDorsi_I_VP3'], (k*Mirror*[0.0593 ; 0.0153 ; 0.0536] - Scapula_CoM) ;... + + [Side 'Scapula_DeltoideusClavicle-P2'],Thorax_osim2antoine.*Mirror*([0.019562226742916928 -0.0065870855556006396 0.01039769231874158]')-Scapula_CoM;... }; @@ -174,11 +227,12 @@ Scapulalocator = varargin{1}; Scapulalocator = Scapulalocator{1, 1}; if Scapulalocator.active - if ~isempty(find(strcmp(Scapulalocator.side,Side),1)) + temp = strfind(Scapulalocator.side,Side); + if ~isempty(temp{:}) vec_1 = Scapula_locator_AA - Scapula_locator_AI; vec_2 = Scapula_locator_TS - Scapula_locator_AI; normal= Sign*cross(vec_1,vec_2)/norm(cross(vec_1,vec_2)); - ind = find(strcmp(Scapulalocator.side,Side),1); + ind = 1; Scapula_locator_AA = Scapula_locator_AA + Scapulalocator.height(ind)*normal*1e-2; Scapula_locator_AI = Scapula_locator_AI + Scapulalocator.height(ind)*normal*1e-2; Scapula_locator_TS = Scapula_locator_TS + Scapulalocator.height(ind)*normal*1e-2; @@ -202,53 +256,28 @@ I_Scapula_generic=[0.0012429 0.0011504 0.0013651 0.0004494 Sign*0.00040922 Sign*0.0002411]; I_Scapula=(norm(Thorax_osim2antoine)^2*Mass.Scapula_Mass/Scapula_Mass_generic)*I_Scapula_generic; -%% "Human_model" structure generation - -num_solid=0; -%% Scapulo-thoracic joint - - - -syms phi lambda real% latitude longitude -x= Thorax_Rx*sin(lambda); -y= -Thorax_Ry*sin(phi)*cos(lambda); - - -z= Mirror(3,3)*Thorax_Rz*cos(phi)*cos(lambda); +%% Scapulo-thoracic joint -PHI = atan((Thorax_Rz*Thorax_Ry*tan(lambda)*(1 - tan(phi)^2))/(Thorax_Rx*Thorax_Ry-Thorax_Rx*Thorax_Rz*tan(phi)^2)); -ALPHA = atan( tan(phi)*(Thorax_Rz*(1 - tan(phi)^2)/(Thorax_Ry- Thorax_Rz*tan(phi)^2) -1)); - +syms theta1 theta2 real % theta1: longitude; theta2: parametric latitude; phi: geocentric latitude +% The coordinates of the Scapula center on the ellipsoid are function of +% longitude and parametric latitude +x = -Thorax_Rx*cos(theta1)*cos(theta2); +y = Thorax_Ry*sin(theta2); +z = Mirror(3,3)*Thorax_Rz*sin(theta1)*cos(theta2); -% ScapuloThoracic_J0 : theta_2 -num_solid=num_solid+1; % solid number -name=list_solid{num_solid}; % solid name -eval(['incr_solid=s_' name ';']) % solid number in model tree -Human_model(incr_solid).name=[Side name]; % solid name with side -Human_model(incr_solid).sister=s_ScapuloThoracic_J1; % Solid's sister -Human_model(incr_solid).child=0; % Solid's child -Human_model(incr_solid).mother=s_mother; % Solid's mother -Human_model(incr_solid).a=[1 0 0]'; -Human_model(incr_solid).joint=1; -Human_model(incr_solid).limit_inf=-Inf; -Human_model(incr_solid).limit_sup=Inf; -Human_model(incr_solid).ActiveJoint=1; -Human_model(incr_solid).m=0; % Reference mass -Human_model(incr_solid).b=[0 0 0]'; % Attachment point position in mother's frame -Human_model(incr_solid).I=zeros(3,3); % Reference inertia matrix -Human_model(incr_solid).c=[0 0 0]'; % Centre of mass position in local frame -Human_model(incr_solid).calib_k_constraint=[]; -Human_model(incr_solid).u=[]; % fixed rotation with respect to u axis of theta angle -Human_model(incr_solid).theta=[]; -Human_model(incr_solid).KinematicsCut=[]; % kinematic cut -Human_model(incr_solid).linear_constraint=[]; -Human_model(incr_solid).Visual=0; +% PHI: Geodetic latitude +PHI = atan(-(Thorax_Rx*Thorax_Rz*tan(theta2)*(1 + tan(theta1)^2))/(Thorax_Rz*Thorax_Ry+Thorax_Rx*Thorax_Ry*tan(theta1)^2)); +% ALPHA: the angle enabling the scapula tangency on the ellipsoid +ALPHA = atan(tan(theta1)*(-Thorax_Ry/Thorax_Rz*sin(PHI)/tan(theta2)-cos(PHI))); +%% "Human_model" structure generation + +num_solid=0; % ScapuloThoracic_J1 num_solid=num_solid+1; % solid number name=list_solid{num_solid}; % solid name @@ -274,9 +303,9 @@ Human_model(incr_solid).Visual=0; % Dependancy Human_model(incr_solid).kinematic_dependancy.active=1; -Human_model(incr_solid).kinematic_dependancy.Joint=[incr_solid-1]; % Thoracicellips +Human_model(incr_solid).kinematic_dependancy.Joint=[incr_solid+3;incr_solid+5]; % ScapuloThoracic_J4bis % Kinematic dependancy function -f_tx = matlabFunction( x,'vars',{lambda}); +f_tx = matlabFunction( x,'vars',{theta1,theta2}); Human_model(incr_solid).kinematic_dependancy.q=f_tx; Human_model(incr_solid).comment='scapulothoracic x regression'; Human_model(incr_solid).FunctionalAngle=[name]; @@ -308,9 +337,9 @@ Human_model(incr_solid).FunctionalAngle=[name]; % Dependancy Human_model(incr_solid).kinematic_dependancy.active=1; -Human_model(incr_solid).kinematic_dependancy.Joint=[incr_solid+2; incr_solid-2]; % Thoracicellips +Human_model(incr_solid).kinematic_dependancy.Joint=[incr_solid+4]; % ScapuloThoracic_J5; ScapuloThoracic_J4bis % Kinematic dependancy function -f_ty = matlabFunction(y,'vars',{phi,lambda}); +f_ty = matlabFunction(y,'vars',{theta2}); Human_model(incr_solid).kinematic_dependancy.q=f_ty; % ScapuloThoracic_J3 @@ -340,9 +369,9 @@ Human_model(incr_solid).comment='scapulothoracic z regression'; % Dependancy Human_model(incr_solid).kinematic_dependancy.active=1; -Human_model(incr_solid).kinematic_dependancy.Joint=[incr_solid+1; incr_solid-3]; % Thoracicellips +Human_model(incr_solid).kinematic_dependancy.Joint=[incr_solid+1; incr_solid+3]; % ScapuloThoracic_J5; ScapuloThoracic_J4bis % Kinematic dependancy function -f_tz = matlabFunction(z,'vars',{phi,lambda}); +f_tz = matlabFunction(z,'vars',{theta1,theta2}); Human_model(incr_solid).kinematic_dependancy.q=f_tz; % ScapuloThoracic_J4 @@ -353,10 +382,10 @@ Human_model(incr_solid).sister=0; % Solid's sister Human_model(incr_solid).child=s_ScapuloThoracic_J5; % Solid's child Human_model(incr_solid).mother=s_ScapuloThoracic_J3; % Solid's mother -Human_model(incr_solid).a=[1 0 0]'; +Human_model(incr_solid).a=[0 1 0]'; %longitude Human_model(incr_solid).joint=1; -Human_model(incr_solid).limit_inf=-pi; -Human_model(incr_solid).limit_sup=pi; +Human_model(incr_solid).limit_inf=-pi/2; +Human_model(incr_solid).limit_sup=pi/2; Human_model(incr_solid).ActiveJoint=1; Human_model(incr_solid).m=0; % Reference mass Human_model(incr_solid).b=[0 0 0]'; % Attachment point position in mother's frame @@ -368,22 +397,26 @@ Human_model(incr_solid).KinematicsCut=[]; % kinematic cut Human_model(incr_solid).linear_constraint=[]; Human_model(incr_solid).Visual=0; -Human_model(incr_solid).FunctionalAngle=[name]; -Human_model(incr_solid).comment='Scapula abduction - adduction'; - +if Sign == 1 + Human_model(incr_solid).FunctionalAngle='Right Scapula abduction(+)/adduction(-)'; + Human_model(incr_solid).comment='Right Scapula abduction(+)/adduction(-)'; +else + Human_model(incr_solid).FunctionalAngle='Left Scapula abduction(-)/adduction(+)'; + Human_model(incr_solid).comment='Left Scapula abduction(-)/adduction(+)'; +end % ScapuloThoracic_J5 num_solid=num_solid+1; % solid number name=list_solid{num_solid}; % solid name eval(['incr_solid=s_' name ';']) % solid number in model tree Human_model(incr_solid).name=[Side name]; % solid name with side -Human_model(incr_solid).sister=0; % Solid's sister +Human_model(incr_solid).sister=s_ScapuloThoracic_J5bis; % Solid's sister Human_model(incr_solid).child=s_ScapuloThoracic_Jalpha; % Solid's child Human_model(incr_solid).mother=s_ScapuloThoracic_J4; % Solid's mother -Human_model(incr_solid).a=[0 1 0]'; +Human_model(incr_solid).a=[0 0 1]'; Human_model(incr_solid).joint=1; -Human_model(incr_solid).limit_inf=-pi/2; -Human_model(incr_solid).limit_sup=pi/2; +Human_model(incr_solid).limit_inf=-Inf; +Human_model(incr_solid).limit_sup=Inf; Human_model(incr_solid).ActiveJoint=1; Human_model(incr_solid).m=0; % Reference mass Human_model(incr_solid).b=[0 0 0]'; % Attachment point position in mother's frame @@ -396,16 +429,44 @@ Human_model(incr_solid).linear_constraint=[]; Human_model(incr_solid).Visual=0; Human_model(incr_solid).FunctionalAngle=[name]; -Human_model(incr_solid).comment='Scapula elevation - depression'; +Human_model(incr_solid).FunctionalAngle=[FullSide 'Scapula elevation(-)/depression(+)']; +Human_model(incr_solid).comment=[FullSide 'Scapula elevation(-)/depression(+)']; % Dependancy Human_model(incr_solid).kinematic_dependancy.active=1; -Human_model(incr_solid).kinematic_dependancy.Joint=[incr_solid-1; incr_solid-5]; % Thoracicellips +Human_model(incr_solid).kinematic_dependancy.Joint=[incr_solid-1; incr_solid+1]; % ScapuloThoracic_J5; ScapuloThoracic_J4bis % Kinematic dependancy function -Ft_PHI= matlabFunction(PHI,'vars',{phi,lambda}); +Ft_PHI= matlabFunction(PHI,'vars',{theta1,theta2}); Human_model(incr_solid).kinematic_dependancy.q=Ft_PHI; -% ScapuloThoracic_alpha_tangence +% ScapuloThoracic_J5bis +num_solid=num_solid+1; % solid number +name=list_solid{num_solid}; % solid name +eval(['incr_solid=s_' name ';']) % solid number in model tree +Human_model(incr_solid).name=[Side name]; % solid name with side +Human_model(incr_solid).sister=0; % Solid's sister +Human_model(incr_solid).child=0; % Solid's child +Human_model(incr_solid).mother=s_ScapuloThoracic_J4; % Solid's mother +Human_model(incr_solid).a=[0 0 1]'; +Human_model(incr_solid).joint=1; +Human_model(incr_solid).limit_inf=-pi/4; +Human_model(incr_solid).limit_sup=pi/4; +Human_model(incr_solid).ActiveJoint=1; +Human_model(incr_solid).m=0; % Reference mass +Human_model(incr_solid).b=[0 0 0]'; % Attachment point position in mother's frame +Human_model(incr_solid).I=zeros(3,3); % Reference inertia matrix +Human_model(incr_solid).c=[0 0 0]'; % Centre of mass position in local frame +Human_model(incr_solid).calib_k_constraint=[]; +Human_model(incr_solid).u=[]; % fixed rotation with respect to u axis of theta angle +Human_model(incr_solid).theta=[]; +Human_model(incr_solid).KinematicsCut=[]; % kinematic cut +Human_model(incr_solid).linear_constraint=[]; +Human_model(incr_solid).Visual=0; +Human_model(incr_solid).comment=[FullSide 'Scapula parametric elevation - depression']; + + + +% ScapuloThoracic_Jalpha num_solid=num_solid+1; % solid number name=list_solid{num_solid}; % solid name eval(['incr_solid=s_' name ';']) % solid number in model tree @@ -413,10 +474,10 @@ Human_model(incr_solid).sister=0; % Solid's sister Human_model(incr_solid).child=s_ScapuloThoracic_J6; % Solid's child Human_model(incr_solid).mother=s_ScapuloThoracic_J5; % Solid's mother -Human_model(incr_solid).a=[1 0 0]'; +Human_model(incr_solid).a=[0 1 0]'; Human_model(incr_solid).joint=1; -Human_model(incr_solid).limit_inf=-pi; -Human_model(incr_solid).limit_sup=pi; +Human_model(incr_solid).limit_inf=-Inf; +Human_model(incr_solid).limit_sup=Inf; Human_model(incr_solid).ActiveJoint=1; Human_model(incr_solid).m=0; % Reference mass Human_model(incr_solid).b=[0 0 0]'; % Attachment point position in mother's frame @@ -432,12 +493,11 @@ Human_model(incr_solid).FunctionalAngle=[name]; % Dependancy Human_model(incr_solid).kinematic_dependancy.active=1; -Human_model(incr_solid).kinematic_dependancy.Joint=[incr_solid-2]; % Thoracicellips +Human_model(incr_solid).kinematic_dependancy.Joint=[incr_solid-3; incr_solid-1]; % ScapuloThoracic_J5 % Kinematic dependancy function -Ft_ALPHA= matlabFunction(ALPHA,'vars',{phi}); +Ft_ALPHA= matlabFunction(ALPHA,'vars',{theta1,theta2}); Human_model(incr_solid).kinematic_dependancy.q=Ft_ALPHA; - % ScapuloThoracic_J6 num_solid=num_solid+1; % solid number name=list_solid{num_solid}; % solid name @@ -445,8 +505,8 @@ Human_model(incr_solid).name=[Side name]; % solid name with side Human_model(incr_solid).sister=0; % Solid's sister Human_model(incr_solid).child=s_Scapula; % Solid's child -Human_model(incr_solid).mother=s_ScapuloThoracic_Jalpha; % Solid's mother -Human_model(incr_solid).a=[0 0 1]'; +Human_model(incr_solid).mother=s_ScapuloThoracic_J5; % Solid's mother +Human_model(incr_solid).a=[-1 0 0]'; Human_model(incr_solid).joint=1; Human_model(incr_solid).limit_inf=-pi/2; Human_model(incr_solid).limit_sup=pi/2; @@ -456,13 +516,13 @@ Human_model(incr_solid).I=zeros(3,3); % Reference inertia matrix Human_model(incr_solid).c=[0 0 0]'; % Centre of mass position in local frame Human_model(incr_solid).calib_k_constraint=[]; -Human_model(incr_solid).u=[]; % fixed rotation with respect to u axis of theta angle +Human_model(incr_solid).u=[]; % fixed rotation with respect to u axis of theta1 angle Human_model(incr_solid).theta=[]; Human_model(incr_solid).KinematicsCut=[]; % kinematic cut Human_model(incr_solid).linear_constraint=[]; Human_model(incr_solid).Visual=0; -Human_model(incr_solid).comment='Scapula upward rotation'; -Human_model(incr_solid).FunctionalAngle=[name]; +Human_model(incr_solid).comment=[FullSide 'Scapula rotation Upward(-)/Downward(+)']; +Human_model(incr_solid).FunctionalAngle=[FullSide 'Scapula rotation Upward(-)/Downward(+)']; % Scapula num_solid=num_solid+1; % solid number @@ -482,100 +542,16 @@ Human_model(incr_solid).I=[I_Scapula(1) I_Scapula(4) I_Scapula(5); I_Scapula(4) I_Scapula(2) I_Scapula(6); I_Scapula(5) I_Scapula(6) I_Scapula(3)]; % Reference inertia matrix Human_model(incr_solid).c=-Scapula_stJointNode; % Centre of mass position in local frame Human_model(incr_solid).calib_k_constraint=[]; -Human_model(incr_solid).u=[]; % fixed rotation with respect to u axis of theta angle -Human_model(incr_solid).theta=[]; +Human_model(incr_solid).u=[]; % fixed rotation with respect to u axis of theta1 angle +Human_model(incr_solid).theta=0; Human_model(incr_solid).KinematicsCut=[]; % kinematic cut Human_model(incr_solid).linear_constraint=[]; Human_model(incr_solid).anat_position=Scapula_position_set; Human_model(incr_solid).Visual=1; Human_model(incr_solid).visual_file=['Holzbaur/Scapula_' lower(Side) '.mat']; -Human_model(incr_solid).comment='Scapula internal rotation'; -Human_model(incr_solid).FunctionalAngle=[name]; +Human_model(incr_solid).comment=[FullSide 'Scapula Internal rotation']; +Human_model(incr_solid).FunctionalAngle=[FullSide 'Scapula Internal rotation']; Human_model(incr_solid).density=1.04; %kg.L-1 - - -% AcromioClavicular Joint - -% % AcromioClavicular_J1 -% num_solid=num_solid+1; % solid number -% name=list_solid{num_solid}; % solid name -% eval(['incr_solid=s_' name ';']) % solid number in model tree -% Human_model(incr_solid).name=[Side name]; % solid name with side -% Human_model(incr_solid).sister=0; % Solid's sister -% Human_model(incr_solid).child=s_AcromioClavicular_J2; % Solid's child -% Human_model(incr_solid).mother=s_Scapula; % Solid's mother -% Human_model(incr_solid).a=[0 1 0]'; -% Human_model(incr_solid).joint=1; -% Human_model(incr_solid).limit_inf=-pi; -% Human_model(incr_solid).limit_sup=pi; -% Human_model(incr_solid).ActiveJoint=1; -% Human_model(incr_solid).m=0; % Reference mass -% Human_model(incr_solid).b=Scapula_acJointNode-Scapula_stJointNode; % Attachment point position in mother's frame -% Human_model(incr_solid).I=zeros(3,3); % Reference inertia matrix -% Human_model(incr_solid).c=[0 0 0]'; % Centre of mass position in local frame -% Human_model(incr_solid).calib_k_constraint=[]; -% Human_model(incr_solid).u=[]; % fixed rotation with respect to u axis of theta angle -% Human_model(incr_solid).theta=[]; -% Human_model(incr_solid).KinematicsCut=[]; % kinematic cut -% Human_model(incr_solid).linear_constraint=[]; -% Human_model(incr_solid).Visual=0; -% Human_model(incr_solid).comment='to be completed'; -% Human_model(incr_solid).FunctionalAngle=[Side name]; -% -% -% % AcromioClavicular_J2 -% num_solid=num_solid+1; % solid number -% name=list_solid{num_solid}; % solid name -% eval(['incr_solid=s_' name ';']) % solid number in model tree -% Human_model(incr_solid).name=[Side name]; % solid name with side -% Human_model(incr_solid).sister=0; % Solid's sister -% Human_model(incr_solid).child=s_AcromioClavicular_J3; % Solid's child -% Human_model(incr_solid).mother=s_AcromioClavicular_J1; % Solid's mother -% Human_model(incr_solid).a=[1 0 0]'; -% Human_model(incr_solid).joint=1; -% Human_model(incr_solid).limit_inf=-pi; -% Human_model(incr_solid).limit_sup=pi; -% Human_model(incr_solid).ActiveJoint=1; -% Human_model(incr_solid).m=0; % Reference mass -% Human_model(incr_solid).b=[0 0 0]'; % Attachment point position in mother's frame -% Human_model(incr_solid).I=zeros(3,3); % Reference inertia matrix -% Human_model(incr_solid).c=[0 0 0]'; % Centre of mass position in local frame -% Human_model(incr_solid).calib_k_constraint=[]; -% Human_model(incr_solid).u=[]; % fixed rotation with respect to u axis of theta angle -% Human_model(incr_solid).theta=[]; -% Human_model(incr_solid).KinematicsCut=[]; % kinematic cut -% Human_model(incr_solid).linear_constraint=[]; -% Human_model(incr_solid).Visual=0; -% Human_model(incr_solid).comment='to be completed'; -% Human_model(incr_solid).FunctionalAngle=[Side name]; -% -% -% % AcromioClavicular_J3 -% num_solid=num_solid+1; % solid number -% name=list_solid{num_solid}; % solid name -% eval(['incr_solid=s_' name ';']) % solid number in model tree -% Human_model(incr_solid).name=[Side name]; % solid name with side -% Human_model(incr_solid).sister=0; % Solid's sister -% Human_model(incr_solid).child=0; % Solid's child -% Human_model(incr_solid).mother=s_AcromioClavicular_J2; % Solid's mother -% Human_model(incr_solid).a=[0 1 0]'; -% Human_model(incr_solid).joint=1; -% Human_model(incr_solid).limit_inf=-pi; -% Human_model(incr_solid).limit_sup=pi; -% Human_model(incr_solid).ActiveJoint=1; -% Human_model(incr_solid).m=0; % Reference mass -% Human_model(incr_solid).b=[0 0 0]'; % Attachment point position in mother's frame -% Human_model(incr_solid).I=zeros(3,3); % Reference inertia matrix -% Human_model(incr_solid).c=[0 0 0]'; % Centre of mass position in local frame -% Human_model(incr_solid).calib_k_constraint=[]; -% Human_model(incr_solid).u=[]; % fixed rotation with respect to u axis of theta angle -% Human_model(incr_solid).theta=[]; -% Human_model(incr_solid).KinematicsCut=[]; % kinematic cut -% Human_model(incr_solid).ClosedLoop=[Side 'Clavicle_AcromioClavicularJointNode']; % if this solid close a closed-loop chain : {number of solid i on which is attached this solid ; attachement point (local frame of solid i} -% Human_model(incr_solid).linear_constraint=[]; -% Human_model(incr_solid).Visual=0; -% Human_model(incr_solid).comment='to be completed'; -% Human_model(incr_solid).FunctionalAngle=[Side name]; - +Human_model(incr_solid).volume_constraints=2*[Thorax_Rx Thorax_Ry Thorax_Rz ; Thorax_Rx Thorax_Ry Thorax_Rz]/2; end diff --git a/Functions/Models/Osteoarticular/UpperTrunk/ModelParts/Thorax_Shoulder.m b/Functions/Models/Osteoarticular/UpperTrunk/ModelParts/Thorax_Shoulder.m index 887e8920..effcf4c4 100644 --- a/Functions/Models/Osteoarticular/UpperTrunk/ModelParts/Thorax_Shoulder.m +++ b/Functions/Models/Osteoarticular/UpperTrunk/ModelParts/Thorax_Shoulder.m @@ -74,8 +74,9 @@ Thorax_C1HatNode = k*[0.026 0.6 0] - CoM_Thorax; Thorax_T1C5 = k*[0.013 0.462 0] - CoM_Thorax; Thorax_ShoulderRightNode = k*[-0.0408 0.1099 0.1929]-Thorax_T12L1JointNode; +Thorax_ShoulderLeftNode = [1 1 -1].*Thorax_ShoulderRightNode; NeckNode=Thorax_C1HatNode; - +Acromion_inThorax = k*[-0.0416 0.1707 0.1853]; % Joints imported and adjusted from (Puchaud et al. 2019) Thorax_osim2antoine = [k k k]; % scaling coef based on shoulder width %Thorax_osim2antoine = [k k Thorax_ShoulderRightNode(3)/0.17]; % scaling coef based on shoulder width @@ -103,6 +104,10 @@ 'Thorax_T12L1JointNode', Thorax_T12L1JointNode'; ... 'R_Thorax_EllipsoidNode', Thorax_EllipsoidRightNode'; ... 'L_Thorax_EllipsoidNode', Thorax_EllipsoidLeftNode'; ... + 'ShoulderRightNode_inThorax', Thorax_ShoulderRightNode';... + 'ShoulderLeftNode_inThorax', Thorax_ShoulderLeftNode';... + 'RightAcromion_inThorax', Acromion_inThorax';... + 'LeftAcromion_inThorax', [1 0 0;0 1 0;0 0 -1]*Acromion_inThorax';... % Muscle paths ['RThorax_r_PECM1-P1'],Thorax_osim2antoine'.*[0.047729456651525642 -0.10400960432311702 0.091178074794605088]'- CoM_Thorax_osim';... ['LThorax_r_PECM1-P1'],Thorax_osim2antoine'.*[0.047729456651525642 -0.10400960432311702 -0.091178074794605088]'- CoM_Thorax_osim';... diff --git a/Functions/Models/Osteoarticular/UpperTrunk/UpperTrunkClavicle.m b/Functions/Models/Osteoarticular/UpperTrunk/UpperTrunkClavicle.m index eaf364db..272015d7 100644 --- a/Functions/Models/Osteoarticular/UpperTrunk/UpperTrunkClavicle.m +++ b/Functions/Models/Osteoarticular/UpperTrunk/UpperTrunkClavicle.m @@ -317,11 +317,11 @@ 'Lclavicle_r_trap_cl_r-P1',[1 0 0; 0 1 0; 0 0 -1]*(Thorax_osim2antoine'.*([-0.0171;0.019;0.0727]));... % Muscles from (Seth et al., 2019) 'LClavicle_TrapeziusClavicle_S-P2',[1 0 0; 0 1 0; 0 0 -1]*(Thorax_osim2antoine'.*([-0.0301;0.0249;0.0910])-CoM_Clavicle_osim');... - 'LScapula_BicepsL_o',[1 0 0; 0 1 0; 0 0 -1]*(Thorax_osim2antoine'.*([-0.03123;-0.02353;-0.01305])-Clavicle2Scapula);... - 'LScapula_BicepsL_via1',[1 0 0; 0 1 0; 0 0 -1]*(Thorax_osim2antoine'.*([-0.02094;-0.01309;-0.00461])-Clavicle2Scapula);... - 'LScapula_BicepsS_o',[1 0 0; 0 1 0; 0 0 -1]*(Thorax_osim2antoine'.*([0.01268;-0.03931;-0.02625])-Clavicle2Scapula);... - 'LScapula_BicepsS_via1',[1 0 0; 0 1 0; 0 0 -1]*(Thorax_osim2antoine'.*([0.00093;-0.06704;-0.01593])-Clavicle2Scapula);... - 'LScapula_Triceps_o',[1 0 0; 0 1 0; 0 0 -1]*(Thorax_osim2antoine'.*([-0.04565;-0.04073;-0.01377])-Clavicle2Scapula);... + 'LScapula_BicepsL_o',[1 0 0; 0 1 0; 0 0 -1]*(Thorax_osim2antoine'.*([-0.03123;-0.02353;-0.01305])-Clavicle2Scapula');... + 'LScapula_BicepsL_via1',[1 0 0; 0 1 0; 0 0 -1]*(Thorax_osim2antoine'.*([-0.02094;-0.01309;-0.00461])-Clavicle2Scapula');... + 'LScapula_BicepsS_o',[1 0 0; 0 1 0; 0 0 -1]*(Thorax_osim2antoine'.*([0.01268;-0.03931;-0.02625])-Clavicle2Scapula');... + 'LScapula_BicepsS_via1',[1 0 0; 0 1 0; 0 0 -1]*(Thorax_osim2antoine'.*([0.00093;-0.06704;-0.01593])-Clavicle2Scapula');... + 'LScapula_Triceps_o',[1 0 0; 0 1 0; 0 0 -1]*(Thorax_osim2antoine'.*([-0.04565;-0.04073;-0.01377])-Clavicle2Scapula');... % % % Scapula Muscle paths from (Puchaud et al., 2019) % 'LScapula_DELT1-P3',[1 0 0; 0 1 0; 0 0 -1]*(Thorax_osim2antoine'.*([0.04347;-0.03252;0.00099])-Clavicle2Scapula');... diff --git a/Functions/MuscleForces/CostFunction/MinMax.m b/Functions/MuscleForces/CostFunction/MinMax.m index 4e0e208e..fd1d9ad4 100644 --- a/Functions/MuscleForces/CostFunction/MinMax.m +++ b/Functions/MuscleForces/CostFunction/MinMax.m @@ -1,4 +1,4 @@ -function [Aopt] = MinMax(A0, Aeq, beq, Amin, Amax, fmincon_options, ~, ~, ~, varargin) +function [Aopt,exitflag] = MinMax(A0, Aeq, beq, Amin, Amax, fmincon_options, ~, ~, ~, varargin) % Optimization used for the force sharing problem: min/max criteria % % Based on: @@ -28,7 +28,7 @@ cost_function = @(A) A(1); % optimization Aeqbis=[zeros(size(Aeq,1),1) Aeq]; -Aopt_inter = fmincon(cost_function,[1;A0],[],[],Aeqbis,beq,[0;Amin],[Inf;Amax],@(A) constraint_minmax(A),fmincon_options); +[Aopt_inter,~,exitflag] = fmincon(cost_function,[1;A0],[],[],Aeqbis,beq,[0;Amin],[Inf;Amax],@(A) constraint_minmax(A),fmincon_options); Aopt=Aopt_inter(2:end,:); end diff --git a/Functions/MuscleForces/CostFunction/PolynomialFunction.m b/Functions/MuscleForces/CostFunction/PolynomialFunction.m index 3515ad8c..44ef6f6e 100644 --- a/Functions/MuscleForces/CostFunction/PolynomialFunction.m +++ b/Functions/MuscleForces/CostFunction/PolynomialFunction.m @@ -1,4 +1,4 @@ -function [Aopt] = PolynomialFunction(A0, Aeq, beq, Amin, Amax, fmincon_options, options, Fa, Fmax, varargin) +function [Aopt,exitflag] = PolynomialFunction(A0, Aeq, beq, Amin, Amax, fmincon_options, options, Fa, Fmax, varargin) % Optimization used for the force sharing problem: polynomial function % % Based on : @@ -39,5 +39,5 @@ cost_function = @(A) (sum((Fa./Fmax(1:ind_act-1)).*A(1:ind_act-1)).^(options) + sum(A(ind_act:end).^2 )); end % Optimization -Aopt = fmincon(cost_function,A0,[],[],Aeq,beq,Amin,Amax,[],fmincon_options); +[Aopt,~,exitflag] = fmincon(cost_function,A0,[],[],Aeq,beq,Amin,Amax,[],fmincon_options); end \ No newline at end of file diff --git a/Functions/MuscleForces/MomentArmsComputationAnalytic.m b/Functions/MuscleForces/MomentArmsComputationAnalytic.m new file mode 100644 index 00000000..20a670e8 --- /dev/null +++ b/Functions/MuscleForces/MomentArmsComputationAnalytic.m @@ -0,0 +1,66 @@ +function [R] = MomentArmsComputationAnalytic(BiomechanicalModel,qval) +% Computation of the moment arms matrix (numerical version) +% +% INPUT +% - Biomechanical model: osteo-articular model (see the Documentation for the structure) +% - q : current coordinates of the model +% - dp: differentiation step +% - Ucall : is a unique call for finding +% OUTPUT +% - R: moment arms matrix at the current frame +%________________________________________________________ +% +% Licence +% Toolbox distributed under GPL 3.0 Licence +%________________________________________________________ +% +% Authors : Antoine Muller, Charles Pontonnier, Pierre Puchaud and +% Georges Dumont +% Modification : Pierre Puchaud +%________________________________________________________ + +% +Muscles=BiomechanicalModel.Muscles; +% nq=numel(qval); + +idxm=find([Muscles.exist]); +nmr=numel(idxm); + +if length(qval)==numel(BiomechanicalModel.OsteoArticularModel(:)) && ~isempty(intersect({BiomechanicalModel.OsteoArticularModel.name},'root0')) + q=qval(1:end-6); %only degrees of freedom of the body, not the floating base. +else + q=qval; +end + + +if isfield(BiomechanicalModel,'Coupling') + C=BiomechanicalModel.Coupling; +else + C= ones(nmr,length(q)); +end +[row,col] = find(C); +R=zeros(nmr,length(q));%init R + + +MatP1P2 = MomentArmP1P2(q); +MatP4P3 = MomentArmP4P3(q); + + +for i=unique(col)' + + liste = find(col==i); + + for k=liste' + + j= row(k) ; % muscle indice + + R(j,i) = - 1/vecnorm(MatP1P2(j,i,:))*reshape(MatP1P2(j,i,:),1,3) * reshape(MatP4P3(j,i,:),3,1); + + end +end + +R = R'; + + + +end \ No newline at end of file diff --git a/Functions/MuscleForces/MomentArmsComputationInit.m b/Functions/MuscleForces/MomentArmsComputationInit.m index b8cb34db..07468523 100644 --- a/Functions/MuscleForces/MomentArmsComputationInit.m +++ b/Functions/MuscleForces/MomentArmsComputationInit.m @@ -27,8 +27,12 @@ nmr=numel(idxm); % -if length(q0)==numel(BiomechanicalModel.OsteoArticularModel(:)) && ~isempty(intersect({BiomechanicalModel.OsteoArticularModel.name},'root0')) - q=q0(1:end-6); %only degrees of freedom of the body, not the floating base. +if ~isempty(intersect({BiomechanicalModel.OsteoArticularModel.name},'root0')) + if length(q0) == numel(BiomechanicalModel.OsteoArticularModel) + q=q0(1:end-6); %only degrees of freedom of the body, not the floating base. + else + q = q0; + end else q=q0; end @@ -139,5 +143,29 @@ BiomechanicalModel.N_pos = N_pos; BiomechanicalModel.M_Bone = M_Bone; BiomechanicalModel.M_pos = M_pos; +BiomechanicalModel.Coupling = C; + +if ~isempty(intersect({BiomechanicalModel.OsteoArticularModel.name},'root0')) + NvBiom = BiomechanicalModel; + NvBiom.OsteoArticularModel= BiomechanicalModel.OsteoArticularModel(1:end-6); +end + + +q = sym('q', [numel(NvBiom.OsteoArticularModel) 1]); % joint coordinates initialization (number of solids - 1 (pelvis)) +assume(q,'real') + + +[P1P2,P4P3] = MomentArmsComputationPreciseSym(NvBiom,q); + + +matlabFunction(P1P2,'file',['Symbolic_function/MomentArmP1P2'],'vars',{q}); +matlabFunction(P4P3,'file',['Symbolic_function/MomentArmP4P3'],'vars',{q}); + + +% +% MomentArmMatrix = MomentArmsComputationPrecise(BiomechanicalModel,q); %depend on reduced set of q (q_red) +% tic() +% matlabFunction(MomentArmMatrix,'file',['Symbolic_function/MomentArmMatrix'],'vars',{q}); +% toc() end diff --git a/Functions/MuscleForces/MomentArmsComputationPreciseSym.m b/Functions/MuscleForces/MomentArmsComputationPreciseSym.m new file mode 100644 index 00000000..35aa0d6b --- /dev/null +++ b/Functions/MuscleForces/MomentArmsComputationPreciseSym.m @@ -0,0 +1,118 @@ +function [P1P2,P4P3] = MomentArmsComputationPreciseSym(BiomechanicalModel,qval) +% Computation of the moment arms matrix (numerical version) +% +% INPUT +% - Biomechanical model: osteo-articular model (see the Documentation for the structure) +% - q : current coordinates of the model +% - dp: differentiation step +% - Ucall : is a unique call for finding +% OUTPUT +% - R: moment arms matrix at the current frame +%________________________________________________________ +% +% Licence +% Toolbox distributed under GPL 3.0 Licence +%________________________________________________________ +% +% Authors : Antoine Muller, Charles Pontonnier, Pierre Puchaud and +% Georges Dumont +% Modification : Pierre Puchaud +%________________________________________________________ +Human_model=BiomechanicalModel.OsteoArticularModel; +Muscles=BiomechanicalModel.Muscles; +% nq=numel(qval); + +idxm=find([Muscles.exist]); +nmr=numel(idxm); + +% +if length(qval)==numel(BiomechanicalModel.OsteoArticularModel(:)) && ~isempty(intersect({BiomechanicalModel.OsteoArticularModel.name},'root0')) + q=qval(1:end-6); %only degrees of freedom of the body, not the floating base. +else + q=qval; +end + +if isfield(BiomechanicalModel,'Coupling') + C=BiomechanicalModel.Coupling; +else + C= ones(nmr,length(q)); +end +[row,col] = find(C); + +NBoneMat = BiomechanicalModel.N_Bone; +NPosMat = BiomechanicalModel.N_pos; +MBoneMat = BiomechanicalModel.M_Bone; +MPosMat = BiomechanicalModel.M_pos; + +%% Computation of moment arms +P1P2 = sym(zeros(nmr,length(q),3));%init +P4P3 = sym(zeros(nmr,length(q),3));%init + +[Human_model.R] = deal(eye(3)); +[Human_model.p] = deal([0 0 0]'); +for j=1:numel(Human_model) + Human_model(j).q=q(j); %#ok<*SAGROW> +end + + + +for i=unique(col)' + + liste = find(col==i); + + for k=liste' + + j= row(k) ; % muscle indice + + + M_Bone = MBoneMat(j,i); + M_pos = MPosMat(j,i); % number of the anatomical landmark in this solid + N_Bone = NBoneMat(j,i); + N_pos = NPosMat(j,i); + + if M_Bone>N_Bone + [solid1,solid2] = find_solid_path(Human_model,M_Bone,N_Bone); + else + [solid2,solid1] = find_solid_path(Human_model,N_Bone,M_Bone); + end + ppac=solid1(1); + + [Human_model] = ForwardPositions(Human_model,ppac); + + + Human_model2 = Human_model; + [Human_model2.R] = deal(zeros(3)); + [Human_model2.p] = deal([0 0 0]'); + + if Human_model2(i).joint == 1 + Human_model2(i).R = Human_model(i).R*wedge(Human_model2(i).a); + else + Human_model2(i).p = Human_model(i).R*Human_model2(i).a; + end + + Human_model2=ForwardPositions(Human_model2,Human_model2(i).child); + + P1 = Human_model(M_Bone).p + Human_model(M_Bone).R * (Human_model(M_Bone).c+ Human_model(M_Bone).anat_position{M_pos,2}); + P2 = Human_model(N_Bone).p + Human_model(N_Bone).R * (Human_model(N_Bone).c + Human_model(N_Bone).anat_position{N_pos,2}); + P3 = Human_model2(N_Bone).p + Human_model2(N_Bone).R*(Human_model(N_Bone).c + Human_model(N_Bone).anat_position{N_pos,2}); + P4 = Human_model2(M_Bone).p + Human_model2(M_Bone).R*(Human_model(M_Bone).c + Human_model(M_Bone).anat_position{M_pos,2}); + + P1P2(j,i,:) = P1 - P2; + P4P3(j,i,:) = P4 - P3; + + end + + +end + + + + +% beware that the matrix is finally nq*nm +%R=R'; + + + + + +end \ No newline at end of file diff --git a/Functions/MuscleForces/Optimization/ForcesComputationOptiNum.m b/Functions/MuscleForces/Optimization/ForcesComputationOptiNum.m index ef3d3bcf..d769ba52 100644 --- a/Functions/MuscleForces/Optimization/ForcesComputationOptiNum.m +++ b/Functions/MuscleForces/Optimization/ForcesComputationOptiNum.m @@ -29,6 +29,8 @@ load([filename '/ExperimentalData.mat']); %#ok time = ExperimentalData.Time; freq = 1/time(2); +addpath('Symbolic_function') + Muscles = BiomechanicalModel.Muscles; load([filename '/InverseKinematicsResults']) %#ok @@ -52,111 +54,84 @@ end %% computation of muscle moment arms from joint posture -% L0=zeros(Nb_muscles,1); -% Ls=zeros(Nb_muscles,1); -% for i=1:Nb_muscles -% L0(i) = BiomechanicalModel.Muscles(i).l0; -% Ls(i) = BiomechanicalModel.Muscles(i).ls; -% end Lmt=zeros(Nb_muscles,Nb_frames); R=zeros(Nb_q,Nb_muscles,Nb_frames); + + for i=1:Nb_frames % for each frames - % Lmt(idm,i) = MuscleLengthComputationNum(BiomechanicalModel,q(:,i)); %dependant of every q (q_complete) - R(:,:,i) = MomentArmsComputationPrecise(BiomechanicalModel,q(:,i)); %depend on reduced set of q (q_red) -% R(:,:,i) = MomentArmsComputationNum(BiomechanicalModel,q(:,i),0.0001); %depend on reduced set of q (q_red) + Lmt(:,i) = MuscleLengthComputationNum(BiomechanicalModel,q(:,i)); %dependant of every q (q_complete) + R(:,:,i) = MomentArmsComputationAnalytic(BiomechanicalModel,q(:,i)); %depend on reduced set of q (q_red) end + + +% mus_flexion = [1,9,13,12]; +% mus_pro = [7,8]; +% num_pro= 42; +% num_flexion = 41; +% R(num_pro,mus_flexion,:)= 0; +% R(num_flexion,mus_pro,:)= 0; +% mus_flexion=mus_flexion+17; +% mus_pro = mus_pro+17; +% num_pro= 49; +% num_flexion = 48; +% R(num_pro,mus_flexion,:)= 0; +% R(num_flexion,mus_pro,:)= 0; idxj=find(sum(R(:,:,1),2)~=0)'; -% Lm = Lmt./(Ls./L0+1); -% % Muscle length ratio to optimal length -% Lm_norm = Lm./L0; -% % Muscle velocity -% Vm = gradient(Lm_norm)*freq; +% idxj=[38:51]; %GH jusque Main %% Computation of muscle forces (optimization) % Optimisation parameters Amin = zeros(Nb_muscles,1); A0 = 0.5*ones(Nb_muscles,1); -Fmax = [Muscles(idm).f0]'; +Fmax = 1*[Muscles(idm).f0]'; Amax = ones(Nb_muscles,1); Fopt = zeros(Nb_muscles,Nb_frames); Aopt = zeros(size(Fopt)); -% Muscle Forces Matrices computation -%[Fa,Fp]=AnalysisParameters.Muscles.MuscleModel(Lm,Vm,Fmax); - % Solver parameters -options1 = optimoptions(@fmincon,'Algorithm','sqp','Display','final','GradObj','off','GradConstr','off','TolFun',1e-6,'MaxIterations',100000,'MaxFunEvals',100000); -options2 = optimoptions(@fmincon,'Algorithm','sqp','Display','final','GradObj','off','GradConstr','off','TolFun',1e-6,'MaxIterations',1000,'MaxFunEvals',2000000); +options1 = optimoptions(@fmincon,'Algorithm','sqp','Display','off','GradObj','off','GradConstr','off','TolFun',1e-6,'MaxIterations',100000,'MaxFunEvals',100000); +options2 = optimoptions(@fmincon,'Algorithm','sqp','Display','off','GradObj','off','GradConstr','off','TolFun',1e-6,'MaxIterations',1000,'MaxFunEvals',2000000); h = waitbar(0,['Forces Computation (' filename ')']); - -[solid_path1,solid_path2,num_solid,num_markers]=Data_ClosedLoop(BiomechanicalModel.OsteoArticularModel); - -dependancies=KinematicDependancy(BiomechanicalModel.OsteoArticularModel); % Closed-loop constraints - -KT=ConstraintsJacobian(BiomechanicalModel,q(:,1),solid_path1,solid_path2,num_solid,num_markers,ones(size(q,1),1),0.0001,dependancies)'; - -% tic(); -% KT2 = FullConstraintsJacobian(BiomechanicalModel,q(:,1),solid_path1,solid_path2,num_solid,num_markers,ones(size(q,1),1),0.0000001,dependancies)'; -% toc(); +KT = Jacobian_closedloop_fullq(q(:,1)); lambda = zeros(size(KT,2),1); -if isempty(lambda) - idxj = [38 39 40 41]; % BO : elbow to hand -else - idxj = 38:47; % BF : elbow to hand -end - - -%G = null(KT(idxj,:)'); -%G=eye(size(KT)); % Moment arms and Active forces -% Aeq = G'*R(idq,:,1).*Fa(:,1)' ; -%Aeq = [G'*R(idxj,:,1).*Fmax' , G'*KT(idxj,:)] ; Aeq = [R(idxj,:,1).*Fmax' , KT(idxj,:)] ; % Joint Torques -%beq = G'*(torques(idq,1) - R(idq,:,1)*Fp(:,1)); beq = torques(idxj,1); % First frame optimization Amin = [Amin; -inf*ones(size(lambda))]; Amax = [Amax; inf*ones(size(lambda))]; [X(:,1)] = AnalysisParameters.Muscles.Costfunction([A0 ; lambda], Aeq, beq, Amin, Amax, options1, AnalysisParameters.Muscles.CostfunctionOptions, Fmax, Fmax); -%[Aopt(:,1)] = AnalysisParameters.Muscles.Costfunction(A0, Aeq, beq, Amin, Amax, options1, AnalysisParameters.Muscles.CostfunctionOptions, Fa(:,1), Fmax); % Muscular activiy Aopt(:,1) = X(1:Nb_muscles,1); A0 = X(:,1); Fopt(:,1) = Fmax.*Aopt(:,1); -%Fopt(:,1) = Fa(:,1).*Aopt(1:Nb_muscles,1)+Fp(:,1); waitbar(1/Nb_frames) for i=2:Nb_frames % for following frames % Closed-loop constraints - KT=ConstraintsJacobian(BiomechanicalModel,q(:,i),solid_path1,solid_path2,num_solid,num_markers,ones(size(q,1),1),0.0001,dependancies)'; - %G = null(KT(idxj,:)'); - %G=eye(size(KT)); + KT = Jacobian_closedloop_fullq(q(:,i)); % Moment arms and Active forces - % Aeq = G'*R(idq,:,i).*Fa(:,i)'; Aeq = [R(idxj,:,i).*Fmax' , KT(idxj,:)] ; % Joint Torques -% beq=G'*(torques(idq,i)- R(idq,:,1)*Fp(:,i)); beq=torques(idxj,i); % Optimization -% [Aopt(:,i)] = AnalysisParameters.Muscles.Costfunction(A0, Aeq, beq, Amin, Amax, options2, AnalysisParameters.Muscles.CostfunctionOptions, Fa(:,i), Fmax); - [X(:,i)] = AnalysisParameters.Muscles.Costfunction(A0, Aeq, beq, Amin, Amax, options1, AnalysisParameters.Muscles.CostfunctionOptions, Fmax, Fmax); + [X(:,i)] = AnalysisParameters.Muscles.Costfunction(A0, Aeq, beq, Amin, Amax, options2, AnalysisParameters.Muscles.CostfunctionOptions, Fmax, Fmax); % Muscular activity Aopt(:,i) = X(1:Nb_muscles,i); A0=X(:,i); Fopt(:,i) = Fmax.*Aopt(:,i); -% Fopt(:,i) = Fa(:,i).*Aopt(1:Nb_muscles,i)+Fp(:,i); waitbar(i/Nb_frames) end diff --git a/README.md b/README.md index 0c54ed5b..3b5f6005 100644 --- a/README.md +++ b/README.md @@ -2,6 +2,8 @@ **CusToM Status:** [![status](http://joss.theoj.org/papers/4b412d584fbfa911edfae882146e2cd3/status.svg)](http://joss.theoj.org/papers/4b412d584fbfa911edfae882146e2cd3) +![alt text](https://github.com/anmuller/CusToM/blob/master/logo_custom.png) + **License:** [![License](https://camo.githubusercontent.com/899066452bb77fd8731295847eb9c17dfdf601d8/68747470733a2f2f696d672e736869656c64732e696f2f62616467652f4c6963656e73652d474e555f47504c76332d6f72616e67652e737667)](https://github.com/anmuller/CusToM/blob/Dev_IRSST/LICENSE)