add matlab scripts

matlab/calc_angle_switch.m

@@ -0,0 +1,11 @@
+function [angle_1,angle_2] = calc_angle_switch(idx,range,X,Y,Z)
+  angle_1 = 0;
+  angle_2 = 0;
+  fitvars = polyfit(Y([idx-range:idx+range]), Z([idx-range:idx+range]), 1);
+  m = fitvars(1)
+  c = fitvars(2)
+  angle_1 = atand(m)
+
+  plot (Y([idx-range:idx+range]),Y([idx-range:idx+range]).*m+c);
+
+end

matlab/calc_finger_pos.m

@@ -0,0 +1,14 @@
+function [org] = calc_finger_pos(angles,trans,translate)
+{
+  org = [0,0,0];
+  org = org + [0,0,trans];
+
+  org = [0,0,translate(1)] + org;
+  org = org * rotx(angles(1));
+
+  org = [0,0,translate(2)] + org;
+  org = org * rotx(angles(2));
+
+  org = [0,0,translate(3)] + org;
+  org = org * rotx(angles(3));
+}

matlab/euklid_dist.m

@@ -0,0 +1,4 @@
+function dist=euklid_dist(a,b)
+  v=abs(a-b);
+  dist = sqrt(sum(v.*v));
+end

matlab/find_next_switch_pos.m

@@ -0,0 +1,26 @@
+function [p1,m1] = find_next_switch_pos(start_pos,X,Y,Z,switch_offset)
+  D = [];
+  for i = [1:length(X)]
+    dist = euklid_dist(start_pos,[X(i),Y(i),Z(i)]);
+    D = [D, dist];
+  end
+  D2=D;
+  start_idx = local_minimas(abs(D));
+  D(start_idx)=0;
+
+  for i = [start_idx+1:length(X)-1]
+    dist = euklid_dist([X(i+1),Y(i+1),Z(i+1)],[X(i),Y(i),Z(i)]);
+    D(i+1)= D(i) + dist;
+  end
+
+  for i = [start_idx-1:-1:2]
+    dist = euklid_dist([X(i-1),Y(i-1),Z(i-1)],[X(i),Y(i),Z(i)]);
+    D(i-1)= D(i) + dist;
+  end
+
+  #plot (D);
+  minimas = local_minimas(abs(D - switch_offset));
+  p1 = minimas(1);
+  m1 = minimas(2);
+
+end

matlab/local_minimas.m

@@ -0,0 +1,8 @@
+function minis = local_minimas( D )
+  minis = []
+  for i = [1:length(D)-2]
+    if D(i) > D(i+1) && D(i+1) < D(i+2)
+      minis = [minis (i+1)]
+    endif
+  end
+end

matlab/start.m

@@ -0,0 +1,107 @@
+clearvars
+clear
+close all
+
+
+  finger_h          = [ 10,       15,    18,      28    ];
+  finger_translate  = [     20,       25,       35      ];
+  finger_min_angles = [     -5,       -5,       -5  ];
+  finger_max_angles = [     85,       90,       70 ];
+  finger_min_translate  = [ 20];
+  finger_max_translate  = [ 0];
+
+
+  switch_offset =20;
+  home_row = 1/3;
+
+  home_row_angles =  finger_min_angles + home_row * (finger_max_angles - finger_min_angles);
+  home_row_trans =  finger_min_translate + home_row * (finger_max_translate - finger_min_translate);
+  home_pos = calc_finger_pos(home_row_angles, home_row_trans, finger_translate);
+
+ X=[];
+ Y=[];
+ Z=[];
+for i = [0:100]/100
+  angle =  finger_min_angles + i * (finger_max_angles - finger_min_angles);
+  trans =  finger_min_translate + i * (finger_max_translate - finger_min_translate);
+  pos = calc_finger_pos(angle,trans, finger_translate);
+
+  X = [X, pos(1)];
+  Y = [Y, pos(2)];
+  Z = [Z, pos(3)];
+end
+
+plot (Y,Z,"p");
+i = [0:100]/100;
+i_f = [0:3600]/3600;
+lin_x = interp1 (i, X, i_f, "spline");
+lin_y = interp1 (i, Y, i_f, "spline");
+lin_z = interp1 (i, Z, i_f, "spline");
+
+figure(1);
+plot (Y, Z, "s", lin_y, lin_z, "r");
+hold on;
+plot (home_pos(2), home_pos(3), "markersize", 75);
+
+axis equal
+
+
+[row_p1,row_m1 ] = find_next_switch_pos(home_pos, X,Y,Z,switch_offset);
+[row_p2,foo ] = find_next_switch_pos([X(row_p1),Y(row_p1),Z(row_p1)], X,Y,Z,switch_offset);
+[row_p3,row_0 ] = find_next_switch_pos([X(row_p2),Y(row_p2),Z(row_p2)], X,Y,Z,switch_offset);
+[foo, row_m2 ] = find_next_switch_pos([X(row_m1),Y(row_m1),Z(row_m1)], X,Y,Z,switch_offset);
+[foo, row_m3 ] = find_next_switch_pos([X(row_m2),Y(row_m2),Z(row_m2)], X,Y,Z,switch_offset);
+
+
+plot (Y(row_0), Z(row_0), "markersize", 50);
+plot (Y(row_p1), Z(row_p1), "markersize", 50);
+plot (Y(row_p2), Z(row_p2), "markersize", 50);
+#plot (Y(row_p3), Z(row_p3), "markersize", 50);
+
+plot (Y(row_m1), Z(row_m1), "markersize", 50);
+plot (Y(row_m2), Z(row_m2), "markersize", 50);
+#plot (Y(row_m3), Z(row_m3), "markersize", 50);
+
+home_angle = calc_angle_switch(row_0,round(abs(row_0-row_m1)/3),X,Y,Z);
+p1_angle = calc_angle_switch(row_p1,round(abs(row_p1-row_0)/3),X,Y,Z);
+p2_angle = calc_angle_switch(row_p2,round(abs(row_p2-row_p1)/3),X,Y,Z);
+p3_angle = calc_angle_switch(row_p3,round(abs(row_p3-row_p2)/3),X,Y,Z);
+
+m1_angle = calc_angle_switch(row_m1,round(abs(row_m1-row_0)/3),X,Y,Z);
+m2_angle = calc_angle_switch(row_m2,round(abs(row_m2-row_m1)/3),X,Y,Z);
+m3_angle = calc_angle_switch(row_m3,round(abs(row_m3-row_m2)/3),X,Y,Z);
+
+#[ foo, row_p1] = min(abs(switch_offset - D));
+
+
+angle_switch_x = [p2_angle, p2_angle,home_angle,m1_angle,m2_angle];
+pos_switch_x = [X(row_p2),X(row_p1),X(row_0),X(row_m1),X(row_m2)];
+pos_switch_y = [Y(row_p2),Y(row_p1),Y(row_0),Y(row_m1),Y(row_m2)];
+pos_switch_z = [Z(row_p2),Z(row_p1),Z(row_0),Z(row_m1),Z(row_m2)];
+plot (Y(row_m1), Z(row_m1), "markersize", 50);
+#plot (Y(row_p1), Z(row_p1), "markersize", 50);
+ % Noisy data
+## x = linspace (0, 2*pi, 100);
+## y = sin (x) + 0.1 * randn (size (x));
+## % Breaks
+## breaks = [0:5, 2*pi];
+## % Constraints: y(0) = 0, y'(0) = 1 and y(3) + y"(3) = 0
+## xc = [0 0 3];
+## yc = [0 1 0];
+## cc = [1 0 1; 0 1 0; 0 0 1];
+## con = struct ("xc", xc, "yc", yc, "cc", cc);
+## % Fit a cubic spline with 8 pieces and constraints
+## pp = splinefit (x, y, 8, "constraints", con);
+## clf;
+## plot (x, y, "s", x, ppval (pp, x), "r", breaks, ppval (pp, breaks), "+r");
+## xlabel ("Independent Variable");
+## ylabel ("Dependent Variable");
+## title ("Fit a cubic spline with constraints");
+## legend ({"data", "fit", "breaks"});
+## axis tight
+## ylim auto
+
+
+
+
+

matlab/test.m

@@ -0,0 +1,11 @@
+xf = [0:0.05:10];
+yf = sin (2*pi*xf/5);
+xp = [0:10];
+yp = sin (2*pi*xp/5);
+lin = interp1 (xp, yp, xf);
+near = interp1 (xp, yp, xf, "nearest");
+pch = interp1 (xp, yp, xf, "pchip");
+spl = interp1 (xp, yp, xf, "spline");
+plot (xf,yf,"r", xf,near,"g", xf,lin,"b", xf,pch,"c", xf,spl,"m",
+      xp,yp,"r*");
+legend ("original", "nearest", "linear", "pchip", "spline");