Homec4science
Diffusion MSM (master) Browse

msm/ShoulderModel_3DOF/load_sharing.m
ece361673202master

Last Change
Raw File

load_sharing.m
View Options

function [F_p,F_s] = load_sharing(SysDyn,W)
%{
Function for solving the indeterminacy of the model (load-sharing).
--------------------------------------------------------------------------
Syntax :
[F_p,F_s] = load_sharing(SysDyn,W)
--------------------------------------------------------------------------
Function Description :
Given the indeterminancy of the resulted equations from the
inverse-dynamics model (SysDyn = W*F), we need to add more information to
end up with a non-trivial solution for F. Here two succesive optimization
is used. In fact they both can be considered as a single optimization. But,
as we can derived the analytical solution for the first unconstrained one
we do it in a successive way. One can look at the problem from the
perspective of solving the Ax=b equations and dividing the solution space
to particular and special answers.
--------------------------------------------------------------------------
%}
% define the weigth matrix
PCSAMat = 1e-2*diag([10 10 10 5 15 15]);
E = PCSAMat^-2;
E = (E+E')/2;
% analytical solution for the first optimization
% the particular answer of the system
F_p = inv(E)*W'*inv(W*inv(E)*W')*SysDyn;
% maximum muscle forces (constraint)
F_max = [40*10;40*10;40*10;40*5;40*15;40*15];
% optmization in the null-space of the W (second optimization)
Null_W = null(W);
P = (Null_W'*E*Null_W) + (Null_W'*E*Null_W)'; % 2*(Null_W'*E*Null_W) originally
% but get rid of the Hessian error
% due to its insymmetricity
q = (2*F_p'*E*Null_W)';
G = [Null_W;-Null_W];
h = [F_max-F_p;F_p];
opts = optimset('Algorithm','interior-point-convex','Display','off');
Mu = quadprog(P,q,G,h,[],[],[],[],[],opts);
F_s = Null_W*Mu; % special answers for plotting
return;
c4science · Help