From bdf35abf09c7a95c1455c408eb0bcf13f12cf65b Mon Sep 17 00:00:00 2001 From: phschoen Date: Sat, 17 Dec 2022 23:49:34 +0100 Subject: [PATCH] add matlab scripts --- matlab/calc_angle_switch.m | 11 ++++ matlab/calc_finger_pos.m | 14 +++++ matlab/euklid_dist.m | 4 ++ matlab/find_next_switch_pos.m | 26 +++++++++ matlab/local_minimas.m | 8 +++ matlab/start.m | 107 ++++++++++++++++++++++++++++++++++ matlab/test.m | 11 ++++ 7 files changed, 181 insertions(+) create mode 100644 matlab/calc_angle_switch.m create mode 100644 matlab/calc_finger_pos.m create mode 100644 matlab/euklid_dist.m create mode 100644 matlab/find_next_switch_pos.m create mode 100644 matlab/local_minimas.m create mode 100644 matlab/start.m create mode 100644 matlab/test.m diff --git a/matlab/calc_angle_switch.m b/matlab/calc_angle_switch.m new file mode 100644 index 0000000..343a482 --- /dev/null +++ b/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 diff --git a/matlab/calc_finger_pos.m b/matlab/calc_finger_pos.m new file mode 100644 index 0000000..d75f55d --- /dev/null +++ b/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)); +} diff --git a/matlab/euklid_dist.m b/matlab/euklid_dist.m new file mode 100644 index 0000000..fef53f0 --- /dev/null +++ b/matlab/euklid_dist.m @@ -0,0 +1,4 @@ +function dist=euklid_dist(a,b) + v=abs(a-b); + dist = sqrt(sum(v.*v)); +end diff --git a/matlab/find_next_switch_pos.m b/matlab/find_next_switch_pos.m new file mode 100644 index 0000000..08cdd7e --- /dev/null +++ b/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 diff --git a/matlab/local_minimas.m b/matlab/local_minimas.m new file mode 100644 index 0000000..686e036 --- /dev/null +++ b/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 diff --git a/matlab/start.m b/matlab/start.m new file mode 100644 index 0000000..05b48dd --- /dev/null +++ b/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 + + + + + diff --git a/matlab/test.m b/matlab/test.m new file mode 100644 index 0000000..d09d8dc --- /dev/null +++ b/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");