Homec4science
Diffusion MSM (master) Browse

msm/ShoulderModel_HHT/contact_model_damped.m
73754adfee53master

Last Change
Raw File

contact_model_damped.m
View Options

function [Dir_contact,Rho_contact,F_contact]= contact_model_damped(Rot_H2T,DRot_H2T,r_HH,Dr_HH,CMDATA)
%{
Function for defining the contact forces at each joint configuration.
--------------------------------------------------------------------------
Syntax :
[Dir_contact,Rho_contact,F_contact]= contact_model(Rot_H2T,r_HH,CMDATA)
--------------------------------------------------------------------------
Function Description :
The function receives the current joint configuration (rotation matrix and
the humeral head translation vector as well as their associated rate of
change) and defines the contact force at each point on the humeral head
surface based on whether the point is in contact or not. We already defined
the points in the humerus body-fixed coordinate frame through
initilize_contact_model().
--------------------------------------------------------------------------
%}
h = CMDATA.h; % receive h_i s in the frame {H}
a = CMDATA.a; % receive associtaed surface area of the h_i s
for i = 1:size(h,1)-1 % associated with longtitude (theta)
for j = 2:size(h,2)-1 % associated with latitude (phi)
h_inT{i,j} = r_HH + Rot_H2T*h{i,j}; % hi in the {T} frame
u(i,j) = norm(h_inT{i,j},2) - CMDATA.r_g; % the penetration of the point hi
% in the glenoid fossa
Dh_inT{i,j} = Dr_HH + DRot_H2T*h{i,j}; % rate of change of hi
Du(i,j) = dot(h_inT{i,j},Dh_inT{i,j})/norm(h_inT{i,j},2); % rate of change of the ui
if gt(u(i,j),0) & lt(u(i,j),0.98*CMDATA.b) % contact condition of the points
F_contact(i,j) = -a(i,j)*CMDATA.s*log(1-u(i,j)/CMDATA.b) + CMDATA.c*Du(i,j);
Dir_contact{i,j} = -h_inT{i,j}/norm(h_inT{i,j},2);
Rho_contact{i,j} = Rot_H2T*h{i,j};
elseif ge(u(i,j),0.98*CMDATA.b)
%u(i,j) = 0.98*b;
F_contact(i,j) = -a(i,j)*CMDATA.s*log(1-0.98) + CMDATA.c*Du(i,j);
Dir_contact{i,j} = -h_inT{i,j}/norm(h_inT{i,j},2);
Rho_contact{i,j} = Rot_H2T*h{i,j};
%disp(['u' num2str(i) ',' num2str(i) '=' num2str(u(i,j)) '>=0.98b']);
else % and if there is no contact there will be no contact force
F_contact(i,j) = 0;
Dir_contact{i,j} = -h_inT{i,j}/norm(h_inT{i,j},2);
Rho_contact{i,j} = Rot_H2T*h{i,j};
end
end
end
% the same calculations for the top and bottom points
% the top point
h_inT{1,1} = r_HH + Rot_H2T*h{1,1}; % hi in the {T} frame
u(1,1) = norm(h_inT{1,1},2) - CMDATA.r_g; % the penetration of the point hi
% in the glenoid fossa
Dh_inT{1,1} = Dr_HH + DRot_H2T*h{1,1}; % rate of change of h1
Du(1,1) = dot(h_inT{1,1},Dh_inT{1,1})/norm(h_inT{1,1},2); % rate of change of the ui
if gt(u(1,1),0) & lt(u(1,1),0.98*CMDATA.b) % contact condition of the points
F_contact(1,1) = -a(1,1)*CMDATA.s*log(1-u(1,1)/CMDATA.b) + CMDATA.c*Du(1,1);
Dir_contact{1,1} = -h_inT{1,1}/norm(h_inT{1,1},2);
Rho_contact{1,1} = Rot_H2T*h{1,1};
elseif ge(u(1,1),0.98*CMDATA.b)
%u(i,j) = 0.98*b;
F_contact(1,1) = -a(1,1)*CMDATA.s*log(1-0.98) + CMDATA.c*Du(1,1);
Dir_contact{1,1} = -h_inT{1,1}/norm(h_inT{1,1},2);
Rho_contact{1,1} = Rot_H2T*h{1,1};
%disp(['u' num2str(1) ',' num2str(1) '=' num2str(u(1,1)) '>=0.98b']);
else % and if there is no contact there will be no contact force
F_contact(1,1) = 0;
Dir_contact{1,1} = -h_inT{1,1}/norm(h_inT{1,1},2);
Rho_contact{1,1} = Rot_H2T*h{1,1};
end
% the bottom point
k = size(h,1); l = size(h,2);
h_inT{k,l} = r_HH + Rot_H2T*h{k,l}; % hi in the {T} frame
u(k,l) = norm(h_inT{k,l},2) - CMDATA.r_g; % the penetration of the point hi
% in the glenoid fossa
Dh_inT{k,l} = Dr_HH + DRot_H2T*h{k,l}; % rate of change of h bottom
Du(k,l) = dot(h_inT{k,l},Dh_inT{k,l})/norm(h_inT{k,l},2); % rate of change of the ui
if gt(u(k,l),0) & lt(u(k,l),0.98*CMDATA.b) % contact condition of the points
F_contact(k,l) = -a(k,l)*CMDATA.s*log(1-u(k,l)/CMDATA.b) + CMDATA.c*Du(k,l);
Dir_contact{k,l} = -h_inT{k,l}/norm(h_inT{k,l},2);
Rho_contact{k,l} = Rot_H2T*h{k,l};
elseif ge(u(k,l),0.98*CMDATA.b)
%u(i,j) = 0.98*b;
F_contact(k,l) = -a(k,l)*CMDATA.s*log(1-0.98) + CMDATA.c*Du(k,l);
Dir_contact{k,l} = -h_inT{k,l}/norm(h_inT{k,l},2);
Rho_contact{k,l} = Rot_H2T*h{k,l};
%disp(['u' num2str(k) ',' num2str(l) '=' num2str(u(k,l)) '>=0.98b']);
else % and if there is no contact there will be no contact force
F_contact(k,l) = 0;
Dir_contact{k,l} = -h_inT{k,l}/norm(h_inT{k,l},2);
Rho_contact{k,l} = Rot_H2T*h{k,l};
end
return
c4science · Help