/**************************************************************** _ _ _ _ __ ___ | |__ ___ | |_ (_) __ ___ ____ _ | '__/ _ \| '_ \ / _ \| __| | |/ _` \ \ / / _` | | | | (_) | |_) | (_) | |_ _ | | (_| |\ V / (_| | |_| \___/|_.__/ \___/ \__(_)/ |\__,_| \_/ \__,_| |__/ Applet Java che presenta un braccio robotico - studio per calcolo del movimento. Copyright 2004 by Umberto Salsi Ultimo aggiornamento: 2004-09-04 Si compila con (ho usato Sun JDK 1.4): javac robot.java Si esegue con: netscape robot.java dove robot.java (questo file) contiene: ****************************************************************/ import java.applet.*; import java.awt.*; /* ____ _ _ | _ \ ___ (_)_ __ | |_ | |_) / _ \| | '_ \| __| | __/ (_) | | | | | |_ |_| \___/|_|_| |_|\__| */ class Point { double[] p = {0.0, 0.0, 0.0}; Point() { p[0] = 0.0; p[1] = 0.0; p[2] = 0.0; } Point(double x, double y, double z) { p[0] = x; p[1] = y; p[2] = z; } double Distance(Point a) { double dx, dy, dz; dx = this.p[0] - a.p[0]; dy = this.p[1] - a.p[1]; dz = this.p[2] - a.p[2]; return dx*dx + dy*dy + dz*dz; } public String toString() { return "("+ p[0]+ ", "+ p[1]+ ", "+ p[2]+ ")"; } } /* __ __ _ \ \ / /__ ___| |_ ___ _ __ \ \ / / _ \/ __| __/ _ \| '__| \ V / __/ (__| || (_) | | \_/ \___|\___|\__\___/|_| */ class Vector { double[] v = {0.0, 0.0, 0.0}; public Vector() { this.v[0] = 0.0; this.v[1] = 0.0; this.v[2] = 0.0; } public Vector(double x, double y, double z) { this.v[0] = x; this.v[1] = y; this.v[2] = z; } public String toString() { return "("+ v[0]+ ", "+ v[1]+ ", "+ v[2]+ ")"; } } /* __ __ _ _ | \/ | __ _| |_ _ __(_)_ __ | |\/| |/ _` | __| '__| \ \/ / | | | | (_| | |_| | | |> < |_| |_|\__,_|\__|_| |_/_/\_\ */ class Matrix { double[][] m = { {1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0} }; public String toString() { int i; String s = "( "; for(i=0; i<3; i++){ s += "("+ m[i][0]+ ", "+ m[i][1]+ ", "+ m[i][2]+ ") "; } return s + " )"; } } /* _____ __ |_ _| __ __ _ ___ / _| | || '__/ _` / __| |_ | || | | (_| \__ \ _| |_||_| \__,_|___/_| */ class Trasf { /*********************************** Sistema delle coordinate: ^ asse y | | ^ asse z | / | / +---------------> asse x ***********************************/ private Matrix R = new Matrix(); private Vector T = new Vector(); public void Trasla(double dx, double dy, double dz) { T.v[0] += dx; T.v[1] += dy; T.v[2] += dz; } private void _ruota(Matrix r) /* Calcola: R=R*r; T=T*r; */ { int i, j; Matrix q = new Matrix(); Vector s = new Vector(); for( i=0; i<3; i++ ) for( j=0; j<3; j++ ) q.m[i][j] = R.m[i][j]; for( i=0; i<3; i++ ) for( j=0; j<3; j++ ) R.m[i][j] = r.m[i][0] * q.m[0][j] + r.m[i][1] * q.m[1][j] + r.m[i][2] * q.m[2][j]; for( i=0; i<3; i++ ) s.v[i] = T.v[i]; for( i=0; i<3; i++ ) T.v[i] = r.m[i][0] * s.v[0] + r.m[i][1] * s.v[1] + r.m[i][2] * s.v[2]; } public void ruota_x(double angolo) { double c, s; Matrix r = new Matrix(); c = Math.cos(angolo); s = Math.sin(angolo); r.m[1][1] = c; r.m[1][2] = s; r.m[2][1] = -s; r.m[2][2] = c; _ruota(r); } public void ruota_y(double angolo) { double c, s; Matrix r = new Matrix(); c = Math.cos(angolo); s = Math.sin(angolo); r.m[0][0] = c; r.m[0][2] = -s; r.m[2][0] = s; r.m[2][2] = c; _ruota(r); } public void ruota_z(double angolo) { double c, s; Matrix r = new Matrix(); c = Math.cos(angolo); s = Math.sin(angolo); r.m[0][0] = c; r.m[0][1] = s; r.m[1][0] = -s; r.m[1][1] = c; _ruota(r); } public void Trasforma(Point p, Point tp) /* Applica la trasformazione di roto-traslazione: tp = R*p + T */ { double x, y, z; x = R.m[0][0] * p.p[0] + R.m[0][1] * p.p[1] + R.m[0][2] * p.p[2] + T.v[0]; y = R.m[1][0] * p.p[0] + R.m[1][1] * p.p[1] + R.m[1][2] * p.p[2] + T.v[1]; z = R.m[2][0] * p.p[0] + R.m[2][1] * p.p[1] + R.m[2][2] * p.p[2] + T.v[2]; tp.p[0] = x; tp.p[1] = y; tp.p[2] = z; } public String toString() { return "R=" + R + ", T=" + T; } } /* _ ____ _ / \ _ __ _ __ ___ | _ \ __ _ _ __ __ _ _ __ ___ ___| |_ ___ _ __ / _ \ | '__| '_ ` _ \ | |_) / _` | '__/ _` | '_ ` _ \ / _ \ __/ _ \ '__| / ___ \| | | | | | | | | __/ (_| | | | (_| | | | | | | __/ || __/ | /_/ \_\_| |_| |_| |_|___|_| \__,_|_| \__,_|_| |_| |_|\___|\__\___|_| |_____| ___ / __| \__ \ |___/ */ class Arm_Parameters { double a1, a2, a3 = 0.0; private static final double gradi = 2.0 * 3.14159 / 360.0; static final double a1min = -90.0*gradi; static final double a1max = +90.0*gradi; static final double a2min = +2.0*gradi; static final double a2max = +135.0*gradi; static final double a3min = +2.0*gradi; static final double a3max = +135.0*gradi; static final double l1 = 1.0; static final double l2 = 5.0; static final double l3 = 4.0; public boolean stopped = false; /* true = moving */ private int moveto_fn = 0; private Point moveto = new Point(); private Vector velocity = new Vector(); public Arm_Parameters() { a1 = 0.0; a2 = 2.0 * gradi; a3 = 2.0 * gradi; //pos = this.GetHandPosition(); //old_pos.p[0] = pos.p[0]; //old_pos.p[1] = pos.p[1]; //old_pos.p[2] = pos.p[2]; } private double range_check(double x, double min, double max) { if( x < min ) return min; if( x > max ) return max; return x; } private Point GetHandPosition(double a1, double a2, double a3) { Trasf h; Point p0, p3; p0 = new Point(0.0, 0.0, 0.0); p3 = new Point(); h = new Trasf(); h.Trasla(l3, 0.0, 0.0); h.ruota_z(a3); h.Trasla(l2, 0.0, 0.0); h.ruota_z(a2); h.Trasla(l1, 0.0, 0.0); h.ruota_x(a1); h.ruota_z(-90.0*gradi); h.Trasforma(p0, p3); return p3; } public Point GetHandPosition() { Trasf h; Point p0, p3; p0 = new Point(0.0, 0.0, 0.0); p3 = new Point(); h = new Trasf(); h.Trasla(l3, 0.0, 0.0); h.ruota_z(a3); h.Trasla(l2, 0.0, 0.0); h.ruota_z(a2); h.Trasla(l1, 0.0, 0.0); h.ruota_x(a1); h.ruota_z(-90.0*gradi); h.Trasforma(p0, p3); return p3; } /****** public Vector GetHandVelocity() { Vector vel = new Vector( old_pos.p[0] - pos.p[0], old_pos.p[1] - pos.p[1], old_pos.p[2] - pos.p[2] ); return vel; } ******/ public void SetHandVelocity(double vx, double vy, double vz) { final double max = 50.0; velocity.v[0] = range_check(vx, -max, max); velocity.v[1] = range_check(vy, -max, max); velocity.v[2] = range_check(vz, -max, max); stopped = true; } public void MoveTo(Point p) { moveto.p[0] = p.p[0]; moveto.p[1] = p.p[1]; moveto.p[2] = p.p[2]; stopped = false; moveto_fn = 1; } public void DoStep() { double da1, da2, da3; double d, d1, d2, d3; Point pos, pos1, pos2, pos3; Point target; /* Soglia di distanza dal target al di sotto della quale ci si deve muovere lentamente (cm): */ final double spdthr = 0.7; if( stopped ) return; //old_pos = pos; pos = GetHandPosition(); if( moveto_fn == 1 ){ SetHandVelocity( moveto.p[0] - pos.p[0], moveto.p[1] - pos.p[1], moveto.p[2] - pos.p[2]); } target = new Point( pos.p[0] + velocity.v[0], pos.p[1] + velocity.v[1], pos.p[2] + velocity.v[2]); d = target.Distance(pos); /* Fin qui stopped==false. Adesso lo metto true e se trovo almeno un movimento che migliora il posizionamento, allora lo rimetto false. */ /* Finezza del movimento: errore = (lungh.braccio) * (variaz.angolo) da cui: (variaz.angolo) = errore / (lungh.braccio) */ if( d < spdthr*spdthr ){ da1 = 0.04 / (l2+l3); da2 = 0.04 / (l2+l3); da3 = 0.04 / l3; } else { da1 = 0.08 / (l2+l3); da2 = 0.04 / (l2+l3); da3 = 0.04 / l3; } /* Tenta di muovere il braccio per migliorare l'avvicinamento al target. Se non e' possibile un ulteriore miglioramento, allora questa e' la migliore approssimazione del posizionamento assoluto, e quindi blocca il moto settando stopped. */ stopped = true; /* a1: */ pos1 = GetHandPosition(range_check(a1+da1, a1min, a1max), a2, a3); pos2 = GetHandPosition(range_check(a1-da1, a1min, a1max), a2, a3); d1 = target.Distance(pos1); d2 = target.Distance(pos2); if( d1 < d && d1 < d2 ){ stopped = false; a1 += da1; d = d1; } else if( d2 < d ){ stopped = false; a1 -= da1; d = d2; } /* a2: */ pos1 = GetHandPosition(a1, range_check(a2+da2, a2min, a2max), a3); pos2 = GetHandPosition(a1, range_check(a2-da2, a2min, a2max), a3); d1 = target.Distance(pos1); d2 = target.Distance(pos2); if( d1 < d && d1 < d2 ){ stopped = false; a2 += da2; d = d1; } else if( d2 < d ){ stopped = false; a2 -= da2; d = d2; } /* a3: */ pos1 = GetHandPosition(a1, a2, range_check(a3+da3, a3min, a3max)); pos2 = GetHandPosition(a1, a2, range_check(a3-da3, a3min, a3max)); d1 = target.Distance(pos1); d2 = target.Distance(pos2); if( d1 < d && d1 < d2 ){ stopped = false; a3 += da3; d = d1; } else if( d2 < d ){ stopped = false; a3 -= da3; d = d2; } a1 = range_check(a1, a1min, a1max); a2 = range_check(a2, a2min, a2max); a3 = range_check(a3, a3min, a3max); if( stopped ){ SetHandVelocity(0.0, 0.0, 0.0); moveto_fn = 0; } } } /* _ _ _ __ ___ | |__ ___ | |_ | '__/ _ \| '_ \ / _ \| __| | | | (_) | |_) | (_) | |_ |_| \___/|_.__/ \___/ \__| */ public class robot extends Applet implements Runnable { private Image offscr = null; private Graphics g; private static final Color bgcolor = Color.white; private static final Color fgcolor = Color.black; private static final Color fggray = Color.gray; private long delay = 100; private Thread thread; private int t = 0; private int hsize, vsize; private Trasf w; private static final double gradi = 2.0 * 3.141592 / 360.0; /* Risoluzione video: */ private static final double SH = 30.0; /* pixel/cm */ private static final double SV = 30.0; /* pixel/cm */ /* Dimensione area applet (cm): */ private double TV_H; private double TV_V; /* Posizione telecamera (cm): */ private double cx; private double cy; private double cz; private FontMetrics fm; /* Griglia di riferimento: */ private static final double latomaglia = 1.0; /* lungh. lato maglia */ private static final int nmaglie = 5; /* griglia 4x4 */ private Point[] G = new Point[nmaglie*nmaglie]; /* Il braccio: */ private Arm_Parameters arm1 = new Arm_Parameters(); private long lastUpdate = 0; private int idx = 0; private Point target; private Point slot; private int hold; void proietta(Point p, Point q) /* Lo schermo e' il piano XY, la telecamera si trova in (cx,cy,cz) ed e' rivolta come (0,0,1): q = intersezione della retta data dai punti p e (cx,cy,cz), con il piano XY. */ { double t, px, py; t = cz / (cz - p.p[2]); px = p.p[0]; py = p.p[1]; q.p[0] = cx + t*(px - cx); q.p[1] = cy + t*(py - cy); q.p[2] = 0.0; } void linea3d( double x1, double y1, double z1, double x2, double y2, double z2) { Point p1 = new Point(x1, y1, z1); Point p2 = new Point(x2, y2, z2); Point q1 = new Point(); Point q2 = new Point(); Point s1 = new Point(); Point s2 = new Point(); w.Trasforma(p1, q1); w.Trasforma(p2, q2); proietta(q1, s1); proietta(q2, s2); g.drawLine( (int) (SH * s1.p[0]), vsize - (int) (SV * s1.p[1]), (int) (SH * s2.p[0]), vsize - (int) (SV * s2.p[1])); } void linea3d(Point a, Point b) { Point a2 = new Point(); Point b2 = new Point(); w.Trasforma(a, a2); proietta(a2, a2); w.Trasforma(b, b2); proietta(b2, b2); g.drawLine( (int) (SH * a2.p[0]), vsize - (int) (SV * a2.p[1]), (int) (SH * b2.p[0]), vsize - (int) (SV * b2.p[1])); } void stringa3d(double x, double y, double z, String s, int align) { double h, v; int shift; Point p = new Point(x, y, z); Point q = new Point(); Point t = new Point(); w.Trasforma(p, q); proietta(q, t); switch( align ){ case 'C': shift = - fm.stringWidth(s) / 2; break; case 'R': shift = - fm.stringWidth(s); break; default: shift = 0; } g.drawString(s, (int) (SH * t.p[0]+0.5) + shift, vsize - (int) (SV * t.p[1]+0.5)); } void stringa3d(Point p, String s, int align) { int shift; Point q = new Point(); switch( align ){ case 'C': shift = - fm.stringWidth(s) / 2; break; case 'R': shift = - fm.stringWidth(s); break; default: shift = 0; } w.Trasforma(p, q); proietta(q, q); g.drawString(s, (int) (SH * q.p[0]+0.5) + shift, vsize - (int) (SV * q.p[1]+0.5)); } void linea(Point a, Point b) { g.drawLine( (int) (SH * a.p[0]), vsize - (int) (SV * a.p[1]), (int) (SH * b.p[0]), vsize - (int) (SV * b.p[1])); } void stringa(Point p, String s, int align) { int len, shift; switch( align ){ case 'C': shift = - fm.stringWidth(s) / 2; break; case 'R': shift = - fm.stringWidth(s); break; default: shift = 0; } g.drawString(s, (int) (SH * p.p[0]+0.5) + shift, vsize - (int) (SV * p.p[1]+0.5)); } void disegna_assi() /* Disegna il sistema di riferimento usando la trasformazione w corrente. */ { Point O = new Point(0.0, 0.0, 0.0); Point Px = new Point(2.0, 0.0, 0.0); Point Py = new Point(0.0, 2.0, 0.0); Point Pz = new Point(0.0, 0.0, 2.0); g.setColor(fggray); linea3d(O, Px); linea3d(O, Py); linea3d(O, Pz); stringa3d(O, "O", 'R'); stringa3d(Px, "x", 'L'); stringa3d(Py, "y", 'R'); stringa3d(Pz, "z", 'L'); } double range_check(double x, double min, double max) { if( x < min ) return min; if( x > max ) return max; return x; } private void NON_USATA_disegna_copyright(Trasf h, double len, double d) { double dx = len/14.0; double dy = d/2.0; double x = -12.0*dx; double y = dy; /* matrice 11*3 di punti dove vado a scrivere: */ Point[] p = new Point[33]; int i; for( i=0; i<33; i++ ){ p[i] = new Point(x+dx*(i%11), y-dy*(i/11), -d); h.Trasforma(p[i], p[i]); } /* "U": */ linea3d(p[0], p[22]); linea3d(p[22], p[23]); linea3d(p[23], p[1]); /* "S": */ linea3d(p[3], p[2]); linea3d(p[2], p[13]); linea3d(p[13], p[14]); linea3d(p[14], p[25]); linea3d(p[25], p[24]); /* "A": */ linea3d(p[26], p[4]); linea3d(p[4], p[5]); linea3d(p[5], p[27]); linea3d(p[15], p[16]); /* "L": */ linea3d(p[6], p[28]); linea3d(p[28], p[29]); /* "S": */ linea3d(p[9], p[8]); linea3d(p[8], p[19]); linea3d(p[19], p[20]); linea3d(p[20], p[31]); linea3d(p[31], p[30]); /* "I": */ linea3d(p[10], p[32]); } private void disegna_link(Trasf h, double len, double d) /* Disegna qualcosa che assomiglia a un link del braccio per dare tridimensionalita' al tutto. Il link va dall'origine al punto (-len,0,0) e ha diametro d. e viene sottoposto alla trasformazione h prima di venire disegnato usando la trasformazione w globale. */ { int i; Point[] p = new Point[8]; // punti della base inferiore: p[0] = new Point(0.0, d, d); p[1] = new Point(0.0, d, -d); p[2] = new Point(0.0, -d, -d); p[3] = new Point(0.0, -d, d); // punti della base superiore: p[4] = new Point(-len, d, d); p[5] = new Point(-len, d, -d); p[6] = new Point(-len, -d, -d); p[7] = new Point(-len, -d, d); // applica h: for( i=0; i<8; i++ ) h.Trasforma(p[i], p[i]); // disegna base inferiore: linea3d(p[0], p[1]); linea3d(p[1], p[2]); linea3d(p[2], p[3]); linea3d(p[3], p[0]); // disegna base superiore: linea3d(p[4], p[5]); linea3d(p[5], p[6]); linea3d(p[6], p[7]); linea3d(p[7], p[4]); // disegna lati: for( i=0; i<4; i++ ) linea3d(p[i], p[i+4]); } void disegna_braccio() /* Disegna il braccio usando la trasformazione w corrente. */ { Trasf h; double dummy1 = 0.0; Point p0, p1, p2, p3; g.setColor(fgcolor); p0 = new Point(0.0, 0.0, 0.0); p3 = new Point(); h = new Trasf(); h.Trasla(arm1.l3, 0.0, 0.0); h.ruota_z(arm1.a3); h.Trasla(arm1.l2, 0.0, 0.0); h.ruota_z(arm1.a2); h.Trasla(arm1.l1, 0.0, 0.0); h.ruota_x(arm1.a1); h.ruota_z(-90.0*gradi); h.Trasforma(p0, p3); disegna_link(h, arm1.l3, 0.2); p2 = new Point(); h = new Trasf(); h.Trasla(arm1.l2, 0.0, 0.0); h.ruota_z(arm1.a2); h.Trasla(arm1.l1, 0.0, 0.0); h.ruota_x(arm1.a1); h.ruota_z(-90.0*gradi); h.Trasforma(p0, p2); disegna_link(h, arm1.l2, 0.35); //disegna_copyright(h, arm1.l2, 0.35); p1 = new Point(); h = new Trasf(); h.Trasla(arm1.l1, 0.0, 0.0); h.ruota_x(arm1.a1); h.ruota_z(-90.0*gradi); h.Trasforma(p0, p1); disegna_link(h, arm1.l1, 0.5); g.setColor(fggray); stringa3d(p0, "0", 'R'); stringa3d(p1, "1", 'R'); stringa3d(p2, "2", 'R'); stringa3d(p3, "3", 'R'); } void disegna_griglia() { int i; g.setColor(fggray); // righe verticali: for( i=0; i 0 ){ Thread.sleep(delay); } } } catch(InterruptedException e) { } } } } /**** FINE! ****/