193

APÉNDICE D. CÓDIGO DEL SIMULADOR CINEMÁTICO El código en Java para el simulador cinemático se presenta en este apartado, siendo cada entidad de código (clase o interfaz) colocada en su respectiva sección.

D.1. Clase Tesis3 import java.awt.*; import java.awt.event.*; import javax.swing.*; import javax.swing.border.*; public class Tesis3 {

// Version 3.0 del simulador

UIManager.setLookAndFeel("com.sun.java.swing.plaf.windows.Wind owsLookAndFeel"); } catch (Exception ex) { System.out.println("Failed loading L&F: "); System.out.println(ex); }

public static void main(String[] args) { Tesis3Frame frame = new Tesis3Frame(); // Creación del frame JFrame.setDefaultLookAndFeelDecorated (true); JDialog.setDefaultLookAndFeelDecorated (true);

// Mostrar frame frame.setVisible(true); } }

try {

D.2. Clase Tesis3Frame import java.awt.*; import java.awt.event.*; import java.io.IOException; import javax.swing.*; import javax.swing.border.*; public class Tesis3Frame extends JFrame { // Version 3.0 del simulador

try { controls = new Controls (parallelRobot, pol, opengl_canvas); } catch (IOException e) { System.err.println(); System.err.println(e.getMessage()); System.exit(1); }

// Declaración de componentes simulator.add (controls, BorderLayout.CENTER); simulator.add (opengl_canvas, BorderLayout.EAST); contentPane.setLayout (new BorderLayout()); contentPane.add (simulator, BorderLayout.NORTH);

private Container contentPane; Plataform parallelRobot; Polygonizer pol; OpenGLCanvas opengl_canvas; Controls controls;

// Declaración de listeners menuFileExit.addActionListener ( new ActionListener() { public void actionPerformed(ActionEvent e) { Tesis3Frame.this.windowClosed(); } } );

// Constructor public Tesis3Frame() { contentPane = this.getContentPane(); JPanel simulator = new JPanel (new BorderLayout ()); JMenuBar menuBar = new JMenuBar(); JMenu menuFile = new JMenu("File"); JMenuItem menuFileExit = new JMenuItem("File");

this.addWindowListener ( new WindowAdapter() { public void windowClosing (WindowEvent e) { Tesis3Frame.this.windowClosed(); } } );

parallelRobot = new Plataform (11.0, 3.4, 7.68, 1.45, 1.62, 7.86, 71.38, Plataform.DEG); pol = new Polygonizer(parallelRobot); pol.start(); opengl_canvas = new OpenGLCanvas (450, 450, parallelRobot, pol); setTitle("Tesis"); setSize(900, 500);

} // Finaliza la aplicación protected void windowClosed() { System.exit(0); }

menuFile.add(menuFileExit); menuBar.add(menuFile); setJMenuBar(menuBar); }

194

D.3. Clase Plataform public class Plataform implements Implicit { // Constantes final static int DEG = 0; final static int RAD = 1; final private int PRECISION = 10; final private int MAX_ITERA = 15; final private double STEP_0 = 0.125; final private double sqrt3_6 = Math.sqrt(3) / 6; final private double [] Sigma = {-2 * Math.PI / 3, -2 * Math.PI / 3, 0, 0, 2 * Math.PI / 3, 2 * Math.PI / 3}; // Propiedades relacionadas con la geometría básica del robot paralelo private double b, d, a, c; private double P, L; // Propiedades relacionadas con la traslación y rotación de la plataforma private double px, py, pz; private double alfa, beta, gamma; // Propiedades relacionadas con la posición actual de los vértices del robot paralelo private Vector [] Base = new Vector [6]; private Vector [] Top = new Vector [6]; private Vector [] Q = new Vector [6]; // Propiedades relacionadas con los ángulos de las flechas private double [] Theta = {0, 0, 0, 0, 0, 0}; private int [] optionTheta = {1, 1, 1, 1, 1, 1}; // Propiedades relacionadas con el eslabón L y las rótulas private double Rot_Max_Angle; private double [] thetaL = new double [6]; private double [] phiL = new double [6]; private double [] fiL = new double [6]; private Rotula [] elbowRot = new Rotula [6]; private Rotula [] topRot = new Rotula [6]; private boolean inRotWorkspace = true; // Variables auxiliares para evitar repeticiones en cálculos private double a_sqrt3, c_sqrt3; // Constructores Plataform () { initializeParameters (5.0, 1.0, 3.0, 1.0, 1.0, 3.0, Math.PI / 4); setTheta (0, 0, 0, 0, 0, 0, DEG); } Plataform (double param_b, double param_d, double param_a, double param_c, double param_p, double param_l, double param_rot_max_angle, int type) { initializeParameters (param_b, param_d, param_a, param_c, param_p, param_l, toRad (param_rot_max_angle, type)); setTheta (0, 0, 0, 0, 0, 0, DEG); } Plataform (int param_b, int param_d, int param_a, int param_c, int param_p, int param_l, int param_rot_max_angle, int type) { initializeParameters ((double) param_b, (double) param_d, (double) param_a, (double) param_c, (double) param_p, (double) param_l, toRad ((double) param_rot_max_angle, type)); setTheta (0, 0, 0, 0, 0, 0, DEG); } // Métodos Set // Relacionados con la traslación de la plataforma void setPx (double param_px) { if (inWorkspace (param_px, py, pz, alfa, beta, gamma)) { if (px != param_px) px = param_px; calculateInverseKinematics (); } }

195

void setPx (int param_px) { setPx ((double) param_px); } void setPy (double param_py) { if (inWorkspace (px, param_py, pz, alfa, beta, gamma)) { if (py != param_py) py = param_py; calculateInverseKinematics (); } } void setPy (int param_py) { setPy ((double) param_py); } void setPz (double param_pz) { if (inWorkspace (px, py, param_pz, alfa, beta, gamma)) { if (pz != param_pz) pz = param_pz; calculateInverseKinematics (); } } void setPz (int param_pz) { setPz ((double) param_pz); } void setPosition (double param_px, double param_py, double param_pz) { if (inWorkspace (param_px, param_py, param_pz, alfa, beta, gamma)) { if (px != param_px) px = param_px; if (py != param_py) py = param_py; if (pz != param_pz) pz = param_pz; calculateInverseKinematics (); } } void setPosition (int param_px, int param_py, int param_pz) { setPosition ((double) param_px, (double) param_py, (double) param_pz); } // Relacionados con la rotación de la plataforma void setAlfa (double param_alfa, int type) { double alfa_temp = toRad (param_alfa, type); if (inWorkspace (px, py, pz, alfa_temp, beta, gamma)) { if (alfa != alfa_temp) alfa = alfa_temp; calculateInverseKinematics (); } } void setAlfa (int param_alfa, int type) { setAlfa ((double) param_alfa, type); } void setBeta (double param_beta, int type) { double beta_temp = toRad (param_beta, type); if (inWorkspace (px, py, pz, alfa, beta_temp, gamma)) { if (beta != beta_temp) beta = beta_temp; calculateInverseKinematics ();

196

} } void setBeta (int param_beta, int type) { setBeta ((double) param_beta, type); } void setGamma (double param_gamma, int type) { double gamma_temp = toRad (param_gamma, type); if (inWorkspace (px, py, pz, alfa, beta, gamma_temp)) { if (gamma != gamma_temp) gamma = gamma_temp; calculateInverseKinematics (); } } void setGamma (int param_gamma, int type) { setGamma ((double) param_gamma, type); } void setOrientation (double param_alfa, double param_beta, double param_gamma, int type) { double alfa_temp = toRad (param_alfa, type); double beta_temp = toRad (param_beta, type); double gamma_temp = toRad (param_gamma, type); if (inWorkspace (px, py, pz, alfa_temp, beta_temp, gamma_temp)) { if (alfa != alfa_temp) alfa = alfa_temp; if (beta != beta_temp) beta = beta_temp; if (gamma != gamma_temp) gamma = gamma_temp; calculateInverseKinematics (); } } void setOrientation (int param_alfa, int param_beta, int param_gamma, int type) { setOrientation ((double) param_alfa, (double) param_beta, (double) param_gamma, type); } // Relacionados con los ángulos de las flechas void setTheta (double param_theta, int num, int type) { double theta_temp = toRad (param_theta, type); if (Theta [num - 1] != theta_temp) Theta [num - 1] = theta_temp; calculateDirectKinematics (); } void setTheta (int param_theta, int num, int type) { setTheta ((double) param_theta, num, type); } void setTheta (double [] param_theta, int type) { double theta_temp; for (int j = 0; j < 6; j++)

{

theta_temp = toRad (param_theta[j], type); if (Theta[j] != theta_temp) { if (j < param_theta.length) Theta [j] = theta_temp; else Theta [j] = 0; } } calculateDirectKinematics (); }

197

void setTheta (double p_t1, double p_t2, double p_t3, double p_t4, double p_t5, double p_t6, int type) { double [] p_Theta = {p_t1, p_t2, p_t3, p_t4, p_t5, p_t6}; setTheta (p_Theta, type); } void setTheta (int p_t1, int p_t2, int p_t3, int p_t4, int p_t5, int p_t6, int type) { double [] p_Theta = {(double) p_t1, (double) p_t2, (double) p_t3, (double) p_t4, (double) p_t5, (double) p_t6}; setTheta (p_Theta, type); } return new Vector (px, py, pz);

void setThetaOption (int num, int opt) { } if ((num >= 1 && num fiLtemp) fiLmax = fiLtemp; if (inRotWorkspace) { fiL[j] = (fiLmin + fiLmax) / 2; } else { fiL[j] = Double.NaN; } } } private double [] calculateFiElbow (double phi_rot, double theta_rot, double sigma) { double A, Bs, Bc, C; double argum_cos, angle_tan; double [] fi_rot = new double [4]; double cP = Math.cos(phi_rot); double cT = Math.cos(theta_rot); double cS = Math.cos(sigma); double sP = Math.sin(phi_rot); double sT = Math.sin(theta_rot); double sS = Math.sin(sigma); A = sP * sT * sS + cP * sT * cS; Bs = -sP * cT * sS - cP * cT * cS; Bc = -sP * cS + cP * sS; C = Math.cos(Rot_Max_Angle); argum_cos = Math.sqrt((Math.pow(C, 2) - Math.pow(A, 2)) / (Math.pow(Bs, 2) + Math.pow(Bc, 2))); angle_tan = Math.atan2(Bs, Bc);

201

fi_rot[0] = (float) -Math.acos(-argum_cos) + angle_tan; fi_rot[1] = (float) -Math.acos(argum_cos) + angle_tan; fi_rot[2] = (float) Math.acos(argum_cos) + angle_tan; fi_rot[3] = (float) Math.acos(-argum_cos) + angle_tan; return fi_rot; } private double [] calculateFiTop (double phi_rot, double theta_rot, double sigma) { double A, Bs, Bc, C; double argum_cos, angle_tan; double [] fi_rot = new double [4]; double cA = Math.cos(alfa); double cB = Math.cos(beta); double cG = Math.cos(gamma); double cP = Math.cos(phi_rot); double cT = Math.cos(theta_rot); double cS = Math.cos(sigma); double sA = Math.sin(alfa); double sB = Math.sin(beta); double sG = Math.sin(gamma); double sP = Math.sin(phi_rot); double sT = Math.sin(theta_rot); double sS = Math.sin(sigma); double cAcG = cA * cG; double cAsG = cA * sG; double cBcG = cB * cG; double cBsG = cB * sG; double sBcG = sB * cG; double sBsG = sB * sG; double sAcBcG = sA * cB * cG; double sAcBsG = sA * cB * sG; double sAsBcG = sA * sB * cG; double sAsBsG = sA * sB * sG; double cPcT = cP * cT; double cPsT = cP * sT; double sPcT = sP * cT; double sPsT = sP * sT; A = sPsT * (cAsG * cS + cAcG * sS) + cPsT * (cBcG * cS + sAsBsG * cS - cBsG * sS + sAsBcG * sS) + cT * (-sBcG * cS + sAcBsG * cS + sBsG * sS + sAcBcG * sS); Bs = sPcT * (-cAcG * sS - cAsG * cS) + cPcT * (cBsG * sS - cBcG * cS - sAsBcG * sS - sAsBsG * cS) + sT * (sBsG * sS - sBcG * cS + sAcBcG * sS + sAcBsG * cS); Bc = sP * (cBsG * sS - cBcG * cS - sAsBcG * sG - sAsBsG * cS) + cP * (cAsG * cS + cAcG * sS); C = Math.cos(Rot_Max_Angle); argum_cos = Math.sqrt((Math.pow(C, 2) - Math.pow(A, 2)) / (Math.pow(Bs, 2) + Math.pow(Bc, 2))); angle_tan = Math.atan2(Bs, Bc); fi_rot[0] = (float) -Math.acos(-argum_cos) + angle_tan; fi_rot[1] = (float) -Math.acos(argum_cos) + angle_tan; fi_rot[2] = (float) Math.acos(argum_cos) + angle_tan; fi_rot[3] = (float) Math.acos(-argum_cos) + angle_tan; return fi_rot; } private void calculateRotState () { double cA = Math.cos(alfa); double cB = Math.cos(beta); double cG = Math.cos(gamma); double sA = Math.sin(alfa); double sB = Math.sin(beta); double sG = Math.sin(gamma); double cAsB = cA * sB; double cAcB = cA * cB; double cAsG = cA * sG; double cAcG = cA * cG; double cBsG = cB * sG; double cBcG = cB * cG; double sBsG = sB * sG; double sBcG = sB * cG; double sAsBsG = sA * sB * sG; double sAsBcG = sA * sB * cG; double sAcBsG = sA * cB * sG; double sAcBcG = sA * cB * cG; double cS, sS; for (int j = 0; j < 6; j++) { cS = Math.cos(Sigma[j]); sS = Math.sin(Sigma[j]); elbowRot[j].nx = new Vector (Math.cos(Sigma[j]), Math.sin(Sigma[j]), 0);

202

elbowRot[j].ny = new Vector (-Math.sin(Sigma[j]), Math.cos(Sigma[j]), 0); elbowRot[j].nz = new Vector (0, 0, 1); topRot[j].nx = new Vector (cS * (cBcG + sAsBsG) + sS * (-cBsG + sAsBcG), cS * (cAsG) + sS * (cAcG), cS * (-sBcG + sAcBsG) + sG * (sBsG + sAcBcG)); topRot[j].ny = new Vector (-sS * (cBcG + sAsBsG) + cS * (-cBsG + sAsBcG), -sS * (cAsG) + cS * (cAcG), -sS * (-sBcG + sAcBsG) + cS * (sBsG + sAcBcG)); topRot[j].nz = new Vector (cAsB, -sA, cAcB); } } // Funciones específicas relacionadas con la cinemática private void calculateInverseKinematics () { double A, B, C, angle_cos, angle_tan; calculateTopVertex (); for (int j = 0; j < 6; j++) { A = 2 * P * (Math.sin(Sigma[j]) * (Top[j].x - Base[j].x) - Math.cos(Sigma[j]) * (Top[j].y - Base[j].y)); B = 2 * P * Top[j].z; C = Math.pow(L, 2) - Math.pow(P, 2) - Math.pow(Top[j].x - Base[j].x, 2) - Math.pow(Top[j].y - Base[j].y, 2) - Math.pow(Top[j].z, 2); angle_cos = Math.acos(C / Math.sqrt(Math.pow(A, 2) + Math.pow(B, 2))); angle_tan = Math.atan2 (A, B);

switch (optionTheta[j]) { case 1: Theta[j] = angle_cos + angle_tan; break; case 2: Theta[j] = -angle_cos + angle_tan; break; } } calculateQVertex (); calculateLAngles (); calculateRotState ();

while ((norma > Math.pow(10, -PRECISION)) && (count_itera 0; j--) gl.glVertex3f((float) robot.getBase(j).x, (float) robot.getBase(j).y, (float) robot.getBase(j).z); gl.glEnd(); // Top Vertex x = Math.cos (robot.getAlfa (robot.RAD)) * Math.sin (robot.getBeta (robot.RAD)); y = -Math.sin (robot.getAlfa (robot.RAD)); z = Math.sqrt (1 - Math.pow(x, 2) - Math.pow(y, 2)); top_normal_up = new Vector (x, y, z); top_normal_down = Vector.scalev (-1, top_normal_up); for (int j = 1; j