Homec4science
Diffusion MSM (master) Browse

msm/ShoulderModel_3DOF/muscle_wrapping.m
35b4c71bda0dmaster

Last Change
Raw File

muscle_wrapping.m
View Options

function [Rho_inT,Dir_inT]= muscle_wrapping(Rot_S2T,Rot_H2T)
%{
Function for defining the muscle paths using the wrapping algorithm.
--------------------------------------------------------------------------
Syntax :
[Rho_inT,Dir_inT]= muscle_wrapping(Rot_S2T,Rot_H2T)
--------------------------------------------------------------------------
Function Description :
Based on the inputted joint configuration this function defines the muscle
path. The path is define geometrically with a single sphere as the wrapping
obstacle. The outputs are the lever arm and the muscle-force directions for
the current joint configuration defined by the rotation matrices.
This part has to be updated if we want the wrapping to adapts for the joint
translations.
--------------------------------------------------------------------------
%}
% muscle originis and insertion in [mm]:
AD_Ao=1e-3*[32 29 -15]; AD_Ai=1e-3*[0 -90 20];
MD_Ao=1e-3*[0 33 2]; MD_Ai=1e-3*[0 -90 20];
PD_Ao=1e-3*[-28 23 -55]; PD_Ai=1e-3*[0 -90 20];
SS_Ao=1e-3*[0 25 -81]; SS_Ai=1e-3*[0 7 26];
IS_Ao=1e-3*[-15 -22 -97]; IS_Ai=1e-3*[-21 8 5];
SC_Ao=1e-3*[2 -23 -99]; SC_Ai=1e-3*[21 5 5];
MusIOMat=[AD_Ao AD_Ai;MD_Ao MD_Ai;
PD_Ao PD_Ai;SS_Ao SS_Ai;
IS_Ao IS_Ai;SC_Ao SC_Ai];
% different radii for the wrapping obstacles (all considered as spheres)
r=1e-3*[30 30 30 26.9 23 22.2];
for i=1:6 % goes through each of the 6 muscles
Ao(:,i)=Rot_S2T*[MusIOMat(i,[1:3])]';
sinalpha(i)=Ao(2,i)/sqrt(Ao(1,i)^2+Ao(2,i)^2);
cosalpha(i)=Ao(1,i)/sqrt(Ao(1,i)^2+Ao(2,i)^2);
sinbeta(i)=(Ao(1,i)*cosalpha(i)+Ao(2,i)*sinalpha(i))/norm(Ao(:,i),2);
cosbeta(i)=Ao(3,i)/norm(Ao(:,i),2);
Ai_inT(:,i)=Rot_H2T*MusIOMat(i,[4:6])';
Ai_inTcop(:,i)=[Ai_inT(1,i)*cosbeta(i)*cosalpha(i)+Ai_inT(2,i)*cosbeta(i)*sinalpha(i)-Ai_inT(3,i)*sinbeta(i);
-Ai_inT(1,i)*sinalpha(i)+Ai_inT(2,i)*cosalpha(i);
Ai_inT(1,i)*sinbeta(i)*cosalpha(i)+Ai_inT(2,i)*sinbeta(i)*sinalpha(i)+Ai_inT(3,i)*cosbeta(i)];
singamma(i)=Ai_inTcop(2,i)/sqrt(Ai_inTcop(1,i)^2+Ai_inTcop(2,i)^2);
cosgamma(i)=Ai_inTcop(1,i)/sqrt(Ai_inTcop(1,i)^2+Ai_inTcop(2,i)^2);
RotMat_C2T([3*i-2:3*i],[1:3])=[cosgamma(i)*cosbeta(i)*cosalpha(i)-singamma(i)*sinalpha(i) -singamma(i)*cosbeta(i)*cosalpha(i)-cosgamma(i)*sinalpha(i) sinbeta(i)*cosalpha(i);
cosgamma(i)*cosbeta(i)*sinalpha(i)+singamma(i)*cosalpha(i) -singamma(i)*cosbeta(i)*sinalpha(i)+cosgamma(i)*cosalpha(i) sinbeta(i)*sinalpha(i);
-cosgamma(i)*sinbeta(i) singamma(i)*sinbeta(i) cosbeta(i)];
cosksi(i)=r(i)/norm(Ao(:,i),2);
AiAo(:,i)=Ao(:,i)-Ai_inT(:,i);
d(:,i)=(1/norm(AiAo(:,i),2))*AiAo(:,i);
digits(7)
DELTA(i)=vpa((d(1,i)*Ao(1,i)+d(2,i)*Ao(2,i)+d(3,i)*Ao(3,i))^2-...
(norm(Ao(:,i),2)^2-r(i)^2));
tt(i)=((Ao(1,i)-Ai_inT(1,i))*Ao(1,i)+(Ao(2,i)-Ai_inT(2,i))*Ao(2,i)...
+(Ao(3,i)-Ai_inT(3,i))*Ao(3,i))/(...
(Ai_inT(1,i)-Ao(1,i))^2+(Ai_inT(2,i)-Ao(2,i))^2+...
(Ai_inT(3,i)-Ao(3,i))^2);
% wrapping condition
if ge(DELTA(i),0)&gt(tt(i),0)&lt(tt(i),1) % muscle should be wrapped.
T0_inC(:,i)=[r(i)*sqrt(1-cosksi(i)^2);0;r(i)*cosksi(i)]; % application point T0
T0_inT(:,i)=RotMat_C2T([3*i-2:3*i],[1:3])*T0_inC(:,i);
Dir_inT(:,i)=(1/norm(Ao(:,i)-T0_inT(:,i),2))*(Ao(:,i)-T0_inT(:,i)); % direction n
else % wrapping is not needed.
T0_inT(:,i)=[Ao(1,i)+(Ai_inT(1,i)-Ao(1,i))*tt(i);
Ao(2,i)+(Ai_inT(2,i)-Ao(2,i))*tt(i);
Ao(3,i)+(Ai_inT(3,i)-Ao(3,i))*tt(i)];
Dir_inT(:,i)=(1/norm(Ao(:,i)-Ai_inT(:,i),2))*(Ao(:,i)-Ai_inT(:,i));
end
Rho_inT(:,i) = T0_inT(:,i); % muscle lever-arms, Dir_inT(:,i) is
% the muscle-force direction
end
return
c4science · Help