83 lines
2.4 KiB
Matlab
83 lines
2.4 KiB
Matlab
%% 3D actuation of all joints %%
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
END = 8;
|
|
GAIN = 1;
|
|
|
|
GAIN_HIP = GAIN;
|
|
GAIN_KNEE = GAIN;
|
|
GAIN_ANKLE = GAIN;
|
|
|
|
% setup timeline, query points
|
|
t = 0 : 1 : END;
|
|
q = 0 : 0.001: END;
|
|
|
|
% vars starting with tr_ are "tracks", or arrays of unprocessed positions
|
|
% over a period of time. The rotational position of a joint is measured
|
|
% in radians. Each array corresponds to a degree of freedom. The length
|
|
% of each array is to be equal to the value of END plus 1.
|
|
|
|
% TRACKS: Left Leg Joints X, Y, Z
|
|
tr_LHipY = [0, -0.7, -1, -1.5, -2, -3, -3, -3, -3]; % positive: move foot back
|
|
tr_LHipX = [0, 0, 0, 0, 0, 0, 0, 0, 0]; % pos: move foot inward
|
|
tr_LHipZ = [0, 0, 0, 0, 0, 0, 0, 0, 0]; % pos: move knee left (c-clockw)
|
|
|
|
tr_LKneeY = [0, 0.7, 1, 1.5, 2, 2, 2, 2, 2]; % pos: bend knee normally
|
|
|
|
tr_LAnkleY = [0, -0.4, -0.6, -0.6, -0.6, -0.6, -0.6, -0.6, -0.6]; % pos: point toes down (pitch)
|
|
tr_LAnkleX = [0, 0, 0, 0, 0, 0, 0, 0, 0]; % pos: right side of foot down
|
|
|
|
% TRACKS: Right Leg Joints X, Y, Z
|
|
tr_RHipY = [0, -0.7, -1, -1.5, -2, -3, -3, -3, -3]; % positive: move leg back
|
|
tr_RHipX = [0, 0, 0, 0, 0, 0, 0, 0, 0]; % pos: mv foot in
|
|
tr_RHipZ = [0, 0, 0, 0, 0, 0, 0, 0, 0]; % pos: mv c-clockw
|
|
|
|
tr_RKneeY = [0, 0.7, 1, 1.5, 2, 2, 2, 2, 2]; % pos: bend knee
|
|
|
|
tr_RAnkleY = [0, -0.4, -0.6, -0.6, -0.6, -0.6, -0.6, -0.6, -0.6]; % pos: toes down
|
|
tr_RAnkleX = [0, 0, 0, 0, 0, 0, 0, 0, 0]; % pos: right side down
|
|
|
|
% interp's, left leg
|
|
it_LHipY = interp1(t, tr_LHipY, q, 'spline')';
|
|
it_LHipZ = interp1(t, tr_LHipZ, q, 'spline')';
|
|
it_LHipX = interp1(t, tr_LHipX, q, 'spline')';
|
|
|
|
it_LKneeY = interp1(t, tr_LKneeY, q, 'spline')';
|
|
|
|
it_LAnkleY = interp1(t, tr_LAnkleY, q, 'spline')';
|
|
it_LAnkleX = interp1(t, tr_LAnkleX, q, 'spline')';
|
|
|
|
% interp's, right leg
|
|
it_RHipY = interp1(t, tr_RHipY, q, 'spline')';
|
|
it_RHipZ = interp1(t, tr_RHipZ, q, 'spline')';
|
|
it_RHipX = interp1(t, tr_RHipX, q, 'spline')';
|
|
|
|
it_RKneeY = interp1(t, tr_RKneeY, q, 'spline')';
|
|
|
|
it_RAnkleY = interp1(t, tr_RAnkleY, q, 'spline')';
|
|
it_RAnkleX = interp1(t, tr_RAnkleX, q, 'spline')';
|
|
|
|
%% RETURN VALUES %%
|
|
|
|
% Left Leg
|
|
ret_LHipY = [q', it_LHipY];
|
|
ret_LHipZ = [q', it_LHipZ];
|
|
ret_LHipX = [q', it_LHipX];
|
|
|
|
ret_LKnee = [q', it_LKneeY];
|
|
|
|
ret_LAnkleY = [q', it_LAnkleY];
|
|
ret_LAnkleX = [q', it_LAnkleX];
|
|
|
|
%%%%%%%%%%%%%%%%%%%%
|
|
|
|
% Right Leg
|
|
ret_RHipY = [q', it_RHipY];
|
|
ret_RHipZ = [q', it_RHipZ];
|
|
ret_RHipX = [q', it_RHipX];
|
|
|
|
ret_RKnee = [q', it_RKneeY];
|
|
|
|
ret_RAnkleY = [q', it_RAnkleY];
|
|
ret_RAnkleX = [q', it_RAnkleX];
|