wip joint work

dat/Limbs1.m

@@ -1,7 +1,7 @@
 %% Bend the Knees %%
 
 END  = 4; % total pts: END+1
-GAIN = 3;
+GAIN = 1;
 
 GAIN_HIP   = GAIN;
 GAIN_KNEE  = GAIN;
@@ -9,28 +9,48 @@ GAIN_ANKLE = GAIN;
 
 % setup vars
 t = 0 : 1 : END;
-% yeah im lost
-pos_hip   = [0, -1, -3, -1, 0];
-pos_knee  = [0, 1, 3, 1, 0];
-pos_ankle = [0, 1, 3, 1, 0];
+
+% 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_hip   = interp1(t, pos_hip, querypts_hip, 'spline')';
-interpts_knee  = interp1(t, pos_knee, querypts_knee, 'spline')';
-interpts_ankle = interp1(t, pos_ankle, querypts_ankle, 'spline')';
+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')';
 
-ret_LHip   = [querypts_hip', interpts_hip];
-ret_LKnee  = [querypts_knee', interpts_knee];
-ret_LAnkle = [querypts_ankle', interpts_ankle];
+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_RHip    = [querypts_hip', interpts_hip];
-ret_RKnee   = [querypts_knee', interpts_knee];
-ret_RAnkle = [querypts_ankle', interpts_ankle];
+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, ':.');