Ski boot walking attachment MATLAB code
From DDL Wiki
(Difference between revisions)
(New page: =gmodel= '''This is the model itself. It calls a few functions as data inputs - namely, makelayers and flexdata_cd_dat.''' ---- function gmodel clear all close all A = [1 1 2 2 5 5]; ...) |
|||
Line 3: | Line 3: | ||
'''This is the model itself. It calls a few functions as data inputs - namely, makelayers and flexdata_cd_dat.''' | '''This is the model itself. It calls a few functions as data inputs - namely, makelayers and flexdata_cd_dat.''' | ||
- | |||
+ | <pre><nowiki> | ||
function gmodel | function gmodel | ||
Line 340: | Line 340: | ||
%} | %} | ||
+ | </nowiki></pre> |
Revision as of 12:02, 21 November 2008
gmodel
This is the model itself. It calls a few functions as data inputs - namely, makelayers and flexdata_cd_dat.
function gmodel clear all close all A = [1 1 2 2 5 5]; B = [3 4 3 4 3 4]; %heel is zero xlen = 12; xres = .25; zlen = 3; ddstep = [0 0]; compression_modifier = [0 0]; xpos = 0:xres:xlen; y0 = spline([0 4 6 10 12],[1.2,1.6,1.8,1.4,1.2],xpos); %initial height body_mass = 70; %kg for j = 1:6 dd = zeros(size(y0)); %deflection rubber_layers = makelayers(A(j),B(j)); %begin step! [foot_theta foot_length ankle_x ankle_y hip_x hip_y_nod lint all_y] = flexdata_cd_dat; hip_y = all_y(:,1)-mean(all_y(:,1)); step_time = .50; theta = linspace(-pi/6,pi/6,round(length(lint)*step_time)); time = linspace(0,step_time,length(theta)); %make subsets of everything (we care about foot contact only here) foot_theta = foot_theta(1:length(theta)); ankle_x = ankle_x(1:length(theta)); ankle_y = ankle_y(1:length(theta)); hip_x = hip_x(1:length(theta)); hip_y_nod = hip_y_nod(1:length(theta))-mean(hip_y_nod(1:length(theta))); hip_y = hip_y(1:length(theta)); theta = foot_theta; %LOL hip_y_adj = hip_y; for i = 3:length(theta) stepsecs = time(i)-time(i-1); %find lengths lens = -((xpos-mean(xpos))*sin(theta(i)))+abs(y0*cos(theta(i)))-abs(dd*cos(theta(i))); %Figure out which layers are contacting the ground (or are very %close to doing so) [lens_sort lens_sort_ix] = sort(lens); incontact = sum(lens>=lens_sort(end)-ddstep(i-1)*compression_modifier(i-1)); contact_indexes = lens_sort_ix(end-incontact+1:end); numcontact(i) = incontact; %Full dispalcement continues until more layers are brought into %contact. Because of the lenient "which in contact" logic, this new contact %theoretically happens towards the end of the timestep, making such %an assumption more reasonable. ddstep(i) = lens_sort(end) - lens_sort(end-incontact); hip_dely_dumb = hip_y(i) - hip_y(i-1); hip_dy_dumb(i) = hip_dely_dumb/stepsecs; %dumb calc of hip y velocity hip_dy2_dumb(i) = (hip_dy_dumb(i) - ((hip_y(i-1) - hip_y(i-2))/stepsecs))/stepsecs; %force applied k = rubber_layers(1,contact_indexes); c = rubber_layers(2,contact_indexes); %force expected based on body CoM motion f_expected = hip_dy2_dumb(i) * body_mass; %verify full compression w0 = (sum(k)/body_mass)^.5; gamma = sum(c)/(2*(sum(k)*body_mass)^.5); wd = w0*(1-gamma)^.5; compression_modifier(i) = stepsecs/wd*1000; if compression_modifier(i) > 1 compression_modifier(i) = 1; elseif compression_modifier(i) <.4 compression_modifier(i) = .4; end %Update displacements of layers dd(contact_indexes) = dd(contact_indexes) + ddstep(i)*compression_modifier(i); %catch layers which will be in contact for most of the step dd(dd>y0*.5) = y0(dd>y0*.5)*.5; %Solve for the vertical velocity of the *device* f_app_k = sum(k ./ body_mass .* dd(contact_indexes)); f_app_c = f_expected - f_app_k; hip_dy_adj(i) = -f_app_c/sum(c/body_mass)/5000; %5000 is a made-up constant, %designed to adjust for lack of accurate dynamic properties. %Thus, results presented are *proportional* to the correct %results. %Keep things from flying off the handle if hip_dy_adj(i) < -10 hip_dy_adj(i) = -10; end %Aggregate dy*time into y hip_y_adj(i) = hip_y_adj(i-1) + hip_dy_adj(i) * stepsecs; end %for i figure(3) plot(time,hip_y,'k') plot(time(1:round(.9*end)+1),hip_y_nod(round(.1*end):end)*.7,'r') hold on plot(time,hip_y+hip_y_adj/10) figure(2) %plot(time,hip_y) hold on plot(time,hip_y_adj) figure(5) hold on plot(time,smooth(hip_dy_adj,21)) end %for =makelayers= '''Makes the physical model of the device's springs and dampers.''' ---- function rubber_layers = makelayers(A,B) %multipliers k0 = 200; c0 = 10; %neoprene soft foam k(1) = .07*k0; c(1) = .6*c0; %r1 = [k1;c1]; %neoprene hard foam k(2) = .15*k0; c(2) = .7*c0; %r2 = [k2;c2]; %neoprene soft sheet k(3) = .6*k0; c(3) = .1*c0; %r3 = [k3;c3]; %neoprene hard sheet k(4) = k0; c(4) = .1*c0; %r4 = [k4;c4]; %silicone sheet k(5) = .12*k0; c(5) = .8*c0; %r5 = [k5;c5]; %which layers where? sel = [repmat([A B],1,24) A]; rubber_layers(1,01:10) = [k(sel(1)) k(sel(2)) k(sel(3)) k(sel(4)) k(sel(5)) k(sel(6)) k(sel(7)) k(sel(8)) k(sel(9)) k(sel(10))]; rubber_layers(1,11:20) = [k(sel(11)) k(sel(12)) k(sel(13)) k(sel(14)) k(sel(15)) k(sel(16)) k(sel(17)) k(sel(18)) k(sel(19)) k(sel(20))]; rubber_layers(1,21:30) = [k(sel(21)) k(sel(22)) k(sel(23)) k(sel(24)) k(sel(25)) k(sel(26)) k(sel(27)) k(sel(28)) k(sel(29)) k(sel(30))]; rubber_layers(1,31:40) = [k(sel(31)) k(sel(32)) k(sel(33)) k(sel(34)) k(sel(35)) k(sel(36)) k(sel(37)) k(sel(38)) k(sel(39)) k(sel(40))]; rubber_layers(1,41:49) = [k(sel(41)) k(sel(42)) k(sel(43)) k(sel(44)) k(sel(45)) k(sel(46)) k(sel(47)) k(sel(48)) k(sel(49))]; rubber_layers(2,01:10) = [c(sel(1)) c(sel(2)) c(sel(3)) c(sel(4)) c(sel(5)) c(sel(6)) c(sel(7)) c(sel(8)) c(sel(9)) c(sel(10))]; rubber_layers(2,11:20) = [c(sel(11)) c(sel(12)) c(sel(13)) c(sel(14)) c(sel(15)) c(sel(16)) c(sel(17)) c(sel(18)) c(sel(19)) c(sel(20))]; rubber_layers(2,21:30) = [c(sel(21)) c(sel(22)) c(sel(23)) c(sel(24)) c(sel(25)) c(sel(26)) c(sel(27)) c(sel(28)) c(sel(29)) c(sel(30))]; rubber_layers(2,31:40) = [c(sel(31)) c(sel(32)) c(sel(33)) c(sel(34)) c(sel(35)) c(sel(36)) c(sel(37)) c(sel(38)) c(sel(39)) c(sel(40))]; rubber_layers(2,41:49) = [c(sel(41)) c(sel(42)) c(sel(43)) c(sel(44)) c(sel(45)) c(sel(46)) c(sel(47)) c(sel(48)) c(sel(49))]; =flexdata_cd_dat= '''Prepares the kinesiologic data; calculates foot motion with the device.''' ---- function [foot_theta foot_length ankle_x ankle_y hip_x hip_y lint all_y] = flexdata_cd_dat lint = linspace(0,1000,1000); hip_height = -10*cos(lint*2*3.1516/1000); %hip_theta = [162 162 170 178 185 193 183 170 160 156 158 162]; hip_theta = [157 164 170 178 185 193 183 170 160 154 156 157]; hip_time = [0 100 200 300 400 500 600 700 800 850 900 1000]; hip_theta_cubic = interp1(hip_time,hip_theta,lint,'spline'); knee_theta = [10 5 15 5 7 45 70 20 10]; knee_time = [0 100 250 450 500 600 750 950 1000]; knee_theta_cubic = interp1(knee_time,knee_theta,lint,'spline'); %ankle_theta = [0 -5 0 22 0 -8 8 0]; %ankle_time = [0 25 50 250 440 600 900 1000]; ankle_theta = [15 15]; ankle_time = [0 1000]; ankle_theta_cubic = interp1(ankle_time,ankle_theta,lint,'spline'); foot_length = 28; shin_length = 56; thigh_length = 70; %{ figure(1) %plot(hip_time,hip_theta) plot(lint,hip_theta_cubic) figure(2) %plot(knee_time,knee_theta) plot(lint,knee_theta_cubic) figure(3) %plot(ankle_time,ankle_theta) plot(lint,ankle_theta_cubic) figure(5) plot(lint,hip_height) %} % % % %% That's the data. Now for the motion. % % pi = 3.1416; twopi = 2*pi; hip_theta_rad = (180-hip_theta_cubic)*pi/180; knee_theta_rad = knee_theta_cubic*pi/180; ankle_theta_rad = ankle_theta_cubic*pi/180; %hip_x(1:1000) = 0; hip_x = linspace(1,170,1000); hip_y = hip_height; knee_x = hip_x + thigh_length.*sin(hip_theta_rad); knee_y = hip_y - thigh_length.*cos(hip_theta_rad); ankle_x = knee_x + shin_length.*sin(hip_theta_rad-knee_theta_rad); ankle_y = knee_y - shin_length.*cos(hip_theta_rad-knee_theta_rad); toe_x = ankle_x + foot_length.*sin(hip_theta_rad-knee_theta_rad+(1.9-ankle_theta_rad)); toe_y = ankle_y - foot_length.*cos(hip_theta_rad-knee_theta_rad+(1.9-ankle_theta_rad)); device_y = [.3 2 .3]; device_x = [0 .5 1]; device_y_cubic = interp1([0 .3 .5 .8 1],[1,1.5,2,1.6,1.2],linspace(0,1,100),'spline'); %device_y_cubic = interp1(device_x, device_y,linspace(0,1,100),'spline'); foot_theta = (hip_theta_rad-knee_theta_rad+(1.9-ankle_theta_rad))'-pi/2; [device_theta, device_phi, device_r] = cart2sph(linspace(0,foot_length,100),-device_y_cubic,zeros(1,100)); device_theta_rot = repmat(device_theta,1000,1) + repmat(foot_theta,1,100); device_r_rot = repmat(device_r,1000,1); [device_x_rot device_y_rot dummy2] = sph2cart(device_theta_rot,zeros(1000,100),device_r_rot); device_y_contact = min(device_y_rot,[],2); device_y_add = min([device_y_rot(:,1) device_y_rot(:,100)],[],2)-device_y_contact; device_y_add(600:1000) = zeros(1,401); %figure(9) %plot(lint,device_y_add) all_x = [hip_x' knee_x' ankle_x' toe_x']; all_y_raw = [hip_y' knee_y' ankle_y' toe_y']; ankle_low = ankle_y.*(ankle_y<toe_y); toe_low = toe_y.*(toe_y<ankle_y); all_y_adjustment = repmat(ankle_low' + toe_low',1,4); all_y = all_y_raw - all_y_adjustment; all_y(600:1000,:) = all_y_raw(600:1000,:) - repmat(linspace(all_y_adjustment(600,1),all_y_adjustment(1000,1),401)',1,4); all_y_device = all_y + repmat(device_y_add,1,4); figure(4) clf step = 40; hold on i = 1; k = 1; while k < 600 plot(all_x(k,:),all_y_device(k,:)) i = i + 1; k = k + step; end %{ figure(6) clf step = 40; hold on i = 1; k = 1; while k < 1000 plot(all_x(k,:),all_y_raw(k,:)) i = i + 1; k = k + step; end figure(8) clf step = 40; hold on i = 1; k = 1; while k < 1000 plot(all_x(k,:),all_y_device(k,:)) i = i + 1; k = k + step; end figure(7) clf hold on plot(all_x(:,1),all_y(:,1)-mean(all_y(:,1)),'-') plot(all_x(:,1),all_y_raw(:,1),'-.') plot(all_x(:,1),all_y_device(:,1)-mean(all_y_device(:,1)),'--') %}