package com.boim.walker; import java.io.*; public class JansenLinkage extends WalkerLinkage { public double _Bx; // location of fixed node B on x axis (usually negative) public double _Ay; // location of crank center on y axis (usually positive) public double _AC; // crank-arm length public double _BD, _BE, _BH; // linkage lengths to fixed node B public double _CD, _CH; // lenfths of linkages to crank arm public double _DE, _EF, _FG, _FH, _GH; // other linkage lengths private double[] _C, _D, _E, _F, _G, _H; // node positions public JansenLinkage() { setDefault(); _C = new double[] {0,0}; _D = new double[] {0,0}; _E = new double[] {0,0}; _F = new double[] {0,0}; _G = new double[] {0,0}; _H = new double[] {0,0}; _badList.load("JansenBadList.dat"); update(); } public void setDefault() { /* Jansen's favorite, as published _Bx = -38; _Ay = 7.8; _AC = 15; _BD = 41.5; _BE = 40.1; _BH = 39.3; _CD = 50; _CH = 61.9; _DE = 55.8; _EF = 39.4; _FG = 65.7; _FH = 36.7; _GH = 49; */ _AC = 17.5; // My favorite, so far, as found from my optimizer // This one has rounder, smoother steps, and seems // to be fairly robust to linkage lock _Ay=6.1; _Bx=-48; _BH=43.2; _EF=46.35; _CD=59; _CH=73.2; _BD=48.6; _BE=46.7; _DE=65.4; _FG=77; _FH=41.4; _GH=56.2; _theta = 0; } public void flip() { _Bx *= -1; } public void setOpt1() { // seeded opt with default, and got: _Bx=-45.570; _Ay=7.146; _AC=17.500; _BD=49.872; _BE=33.472; _BH=48.301; _CD=59.833; _CH=76.936; _DE=69.001; _EF=48.221; _FG=78.090; _FH=42.482; _GH=50.628; /* // seeded with above, got 1.16e18 _Bx=-42.013; _Ay=7.904; _AC=17.500; _BD=51.584; _BE=31.930; _BH=51.379; _CD=43.823; _CH=76.623; _DE=76.493; _EF=34.393; _FG=83.158; _FH=33.320; _GH=55.618; // seeded with above, got 3.6e19 _Bx=-40.401; _Ay=9.096; _AC=17.500; _BD=50.204; _BE=32.356; _BH=51.437; _CD=42.862; _CH=75.353; _DE=76.328; _EF=34.954; _FG=83.112; _FH=32.721; _GH=56.818; // seeded with above, 2.16e20 _Bx=-41.051; _Ay=9.312; _AC=17.500; _BD=50.203; _BE=32.282; _BH=51.058; _CD=43.050; _CH=75.663; _DE=76.370; _EF=35.113; _FG=83.085; _FH=32.406; _GH=56.979; */ } // increase/decrease current size by given scale factor public void scale(double s) { _Bx *= s; _Ay *= s; _AC *= s; _BD *= s; _BE *= s; _BH *= s; _CD *= s; _CH *= s; _DE *= s; _EF *= s; _FG *= s; _FH *= s; _GH *= s; } public double check() { // code to check realizability of geometry. // return >1 for HOW unreasonable the geometry is. // NORMALIZED distance to closest "bad" configuration double dist = (_badList==null)?1.0: _badList.closest(getState()) / _badListThresh; if (dist >= 1.0) return(1.0); if (dist < 1e-4) return(1e8); return(1.0/(dist*dist)); } public double undesirable() { double d = 0; // Give a number (in cm... basically) showing how far // out of "desirable" some node locations are at this angle // If foot crosses axis, this is bad. double x = _G[0]; if (_Bx > 0) x = -x; x += 2; if (x > 0) d += x; // If foot gets higher than any node, this is undesirable. // most often occurs with H and F x = _G[1] - _F[1]; if (x > 0) d += x; x = _G[1] - _H[1]; if (x > 0) d += x; return d; } public double[] getPos(char node) { double[] p = {0,0}; switch(node) { case 'A': p[1] = _Ay; break; case 'B': p[0] = _Bx; break; case 'C': p=_C; break; case 'D': p=_D; break; case 'E': p=_E; break; case 'F': p=_F; break; case 'G': p=_G; break; case 'H': p=_H; break; default: System.err.print(node); System.err.println(" is not a valid node name [A..H]"); } return p; } public void printOrbit(int nSteps, PrintStream f) { for (int i=0; i < nSteps; i++) { update(2 * Math.PI * i / nSteps); double[] g = getPos('G'); f.format("%.3f\t%.3f%n",g[0],g[1]); } } public void printOrbitFull(int nSteps, PrintStream f) { for (int i=0; i < nSteps; i++) { update(2 * Math.PI * i / nSteps); for (char node = 'A'; node <= 'H'; node++) { double[] a = getPos(node); if (node != 'A') f.print('\t'); f.format("%.3f %.3f",a[0],a[1]); } f.println(); } } public void print(PrintStream f) { f.format("_Bx=%.3f;%n_Ay=%.3f;%n",_Bx,_Ay); f.format("_AC=%.3f;%n_BD=%.3f;%n_BE=%.3f;%n_BH=%.3f;%n",_AC,_BD, _BE, _BH); f.format("_CD=%.3f;%n_CH=%.3f;%n_DE=%.3f;%n_EF=%.3f;%n_FG=%.3f;%n_FH=%.3f;%n_GH=%.3f;%n", _CD, _CH, _DE, _EF, _FG, _FH, _GH); } // compute all node positions given current linkage lengths, fixed points, and crankshaft angle public void update() { _C[0] = _AC * Math.cos(_theta); _C[1] = _AC * Math.sin(_theta) + _Ay; double[] B = {_Bx,0}; try { if (_Bx > 0) { _D = triVertex(_C,_CD, B,_BD); _E = triVertex(_D,_DE, B,_BE); _H = triVertex( B,_BH,_C,_CH); _F = triVertex(_E,_EF,_H,_FH); _G = triVertex(_F,_FG,_H,_GH); } else { _D = triVertex( B,_BD,_C,_CD); _E = triVertex( B,_BE,_D,_DE); _H = triVertex(_C,_CH, B,_BH); _F = triVertex(_H,_FH,_E,_EF); _G = triVertex(_H,_GH,_F,_FG); } } catch(Exception e) { //System.err.println(e.getMessage()); _G[0] = _G[1] = 0; // show foot at imposible location for err } } public void setState(double[] x) { //_AC = _crankLen; // assume _AC is a constant w.r.t. state _Ay = x[0]; _Bx = x[1]; _BD = x[2]; _BE = x[3]; _BH = x[4]; _CD = x[5]; _CH = x[6]; _DE = x[7]; _EF = x[8]; _FG = x[9]; _FH = x[10]; _GH = x[11]; } public double[] getState() { double[] x = new double[12]; x[0] = _Ay; x[1] = _Bx; x[2] = _BD; x[3] = _BE; x[4] = _BH; x[5] = _CD; x[6] = _CH; x[7] = _DE; x[8] = _EF; x[9] = _FG; x[10]= _FH; x[11]= _GH; return x; } // check how far links are from bumping into each other. // for now, just dist from CH to B axle public double clearanceDistance() { double chx = _H[0] - _C[0]; // vector from C to H double chy = _H[1] - _C[1]; double cbx = _Bx - _C[0]; // vector from C to B axle center double cby = - _C[1]; double chxHat = chx/_CH; double chyHat = chy/_CH; double dp = cbx*chxHat + cby*chyHat; // dot prod of cb with chHat == projection dist on ch double cpx = _C[0] + dp * chxHat; // closest point on ch to axle double cpy = _C[1] + dp * chyHat; cpx -= _Bx; // cp now vector from closest point to B axis double d = cpx*cpx + cpy*cpy; d = Math.sqrt(d); return d; } }