57 lines
1.5 KiB
Matlab
57 lines
1.5 KiB
Matlab
%% Bend the Knees %%
|
|
|
|
END = 4; % total pts: END+1
|
|
GAIN = 1;
|
|
|
|
GAIN_HIP = GAIN;
|
|
GAIN_KNEE = GAIN;
|
|
GAIN_ANKLE = GAIN;
|
|
|
|
% setup vars
|
|
t = 0 : 1 : END;
|
|
|
|
% set positions
|
|
pos_lhip = [ 0, 0, -0.083, 0, 0 ];
|
|
pos_rhip = [ 0, 0, 0, 0, 0 ];
|
|
|
|
pos_lknee = [ 0, 0, 0, 0, 0 ];
|
|
pos_rknee = [ 0, 0, 0, 0, 0 ];
|
|
|
|
pos_lankle = [ 0, 0, 0, 0, 0 ];
|
|
pos_rankle = [ 0, 0, 0, 0, 0 ];
|
|
|
|
|
|
% generate graph lines
|
|
querypts_hip = 0 : 0.001 : END;
|
|
querypts_knee = 0 : 0.001 : END;
|
|
querypts_ankle = 0 : 0.001 : END;
|
|
|
|
interpts_lhip = interp1(t, pos_lhip, querypts_hip, 'spline')';
|
|
interpts_lknee = interp1(t, pos_lknee, querypts_knee, 'spline')';
|
|
interpts_lankle = interp1(t, pos_lankle, querypts_ankle, 'spline')';
|
|
|
|
interpts_rhip = interp1(t, pos_rhip, querypts_hip, 'spline')';
|
|
interpts_rknee = interp1(t, pos_rknee, querypts_knee, 'spline')';
|
|
interpts_rankle = interp1(t, pos_rankle, querypts_ankle, 'spline')';
|
|
|
|
ret_LHip = [querypts_hip', interpts_lhip];
|
|
ret_LKnee = [querypts_knee', interpts_lknee];
|
|
ret_LAnkle = [querypts_ankle', interpts_lankle];
|
|
|
|
ret_RHip = [querypts_hip', interpts_rhip];
|
|
ret_RKnee = [querypts_knee', interpts_rknee];
|
|
ret_RAnkle = [querypts_ankle', interpts_rankle];
|
|
|
|
% create graph
|
|
%figure
|
|
%plot(t, p,'o', querypts, interpts, ':.');
|
|
|
|
% figure % Both Hips
|
|
% plot(t, pos_hip, 'r', querypts_hip, interpts_hip, ':.');
|
|
|
|
% figure % Both Knees
|
|
% plot(t, pos_knee, 'b', querypts_knee, interpts_knee, ':.');
|
|
|
|
% figure % Both Ankles
|
|
% plot(t, pos_ankle, 'g', querypts_ankle, interpts_ankle, ':.');
|