/* * Demo.java - * Copyright (c) 2005 Carmine Lia * e-mail: carmine.lia@libero.it * web-address: http://www.intermedia.sa.it/lia * This program is free software; you can redistribute it and/or * modify it under the terms of the GNU General Public License * as published by the Free Software Foundation; either version 2 * of the License, or any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program; if not, write to the Free Software * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * */ import java.io.IOException; // import java.net.URI; import java.net.URISyntaxException; import javax.robotics.engine.robots.DHRobot; import javax.robotics.engine.robots.Robot; import javax.robotics.vecmath.RMatrix4d; import javax.robotics.vecmath.RVector; import org.apache.xmlbeans.XmlException; /** * Robot class functions demonstration. * @author Carmine Lia * */ public class Demo { public static void main(String[] args) throws XmlException, IOException, URISyntaxException { new Demo(); } private Demo() throws XmlException, IOException, URISyntaxException { final String newLine = System.getProperty("line.separator"); final ClassLoader cl = this.getClass().getClassLoader(); final double[] qr = { Math.PI / 4.0, Math.PI / 4.0, Math.PI / 4.0, Math.PI / 4.0, Math.PI / 4.0, Math.PI / 4.0 }; final double[] qs = { Math.PI / 16.0, 1.0, 1.0, 1.0, 1.0, 1.0 }; // array conf robot data final double[] PUMA560_data = { 0.0, 0.0, 0.0, 0.0, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.35, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.4318, 0.0, 0.0, 0.0, 0.0, 17.4, -0.3638, 0.006, 0.2275, 0.13, 0.0, 0.0, 0.524, 0.0, 0.539, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.15005, 0.0203, -Math.PI / 2.0, 0.0, 0.0, 0.0, 4.8, -0.0203, -0.0141, 0.07, 0.066, 0.0, 0.0, 0.086, 0.0, 0.0125, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.4318, 0.0, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.82, 0.0, 0.019, 0.0, 0.0018, 0.0, 0.0, 0.0013, 0.0, 0.0018, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -Math.PI / 2.0, 0.0, 0.0, 0.0, 0.34, 0.0, 0.0, 0.0, 0.0003, 0.0, 0.0, 0.0004, 0.0, 0.0003, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.09, 0.0, 0.0, 0.032, 0.00015, 0.0, 0.0, 0.00015, 0.0, 0.00004, 0.0, 0.0, 0.0, 0.0, 0.0 }; // array conf robot data final double[] PUMA560_data0 = { 0.0, 0.0, 0.0, 0.0, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.35, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.4318, 0.0, 0.0, 0.0, 0.0, 17.4, -0.3638, 0.006, 0.2275, 0.13, 0.0, 0.0, 0.524, 0.0, 0.539, 0.0, 0.0, 0.0, 0.15005, 0.0203, -Math.PI / 2.0, 0.0, 0.0, 0.0, 4.8, -0.0203, -0.0141, 0.07, 0.066, 0.0, 0.0, 0.086, 0.0, 0.0125, 0.0, 0.0, 0.0, 0.4318, 0.0, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.82, 0.0, 0.019, 0.0, 0.0018, 0.0, 0.0, 0.0013, 0.0, 0.0018, 0.0, 0.0, 0.0, 0.0, 0.0, -Math.PI / 2.0, 0.0, 0.0, 0.0, 0.34, 0.0, 0.0, 0.0, 0.0003, 0.0, 0.0, 0.0004, 0.0, 0.0003, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.09, 0.0, 0.0, 0.032, 0.00015, 0.0, 0.0, 0.00015, 0.0, 0.00004, 0.0 }; // array conf robot data final double[] PUMA560_motor = { 200e-6, 62.6111, 1.48e-3, (.395 + .435) / 2.0, 200e-6, 107.815, .817e-3, (.126 + .071) / 2.0, 200e-6, 53.7063, 1.38e-3, (.132 + .105) / 2.0, 33e-6, 76.0364, 71.2e-6, (11.2e-3 + 16.9e-3) / 2.0, 33e-6, 71.923, 82.6e-6, (9.26e-3 + 14.5e-3) / 2.0, 33e-6, 76.686, 36.7e-6, (3.96e-3 + 10.5e-3) / 2.0 }; final long startTime = System.nanoTime(); // create robot loading configuration file from the local computer // Robot puma = new DHRobot(cl.getResource("example/conf/puma560.xml").toURI()); // create robot loading configuration file from the web Robot puma = new DHRobot(new URI("http://www.intermedia.sa.it/lia/jroboop/puma560.xml")); //int dof = puma.getDof(); /* -------------- KINEMATICS --------------*/ System.out .println("=====================================================" + newLine + "Robot "+ puma.getRobotConfiguration().getName() +" kinematics" + newLine + "=====================================================" + newLine); System.out.println("Robot joints variables" + newLine + puma.getQ().toString("%8.3f" + newLine)); System.out.println(newLine + "Robot position" + newLine + puma.kine().toString("%8.3f")); puma.setQ(qr); System.out.println(newLine + "Robot joints variables" + newLine + puma.getQ().toString("%8.3f" + newLine)); System.out.println(newLine + "Robot position" + newLine + puma.kine().toString("%8.3f")); System.out.println(newLine + "Robot jacobian w/r to base frame" + newLine + puma.jacobian().toString("%8.3f")); System.out.println(newLine + "Robot jacobian w/r to tool frame" + newLine + puma.jacobian(qr.length).toString("%8.3f")); for (int i = 1; i <= qr.length; i++) { System.out .println(newLine + "Robot position partial derivative with respect to joint " + i + newLine + puma.dTdqi(i).toString("%8.3f")); } final RMatrix4d T = new RMatrix4d(puma.kine()); final RVector vs = new RVector(qs); final RVector vr = new RVector(qr); System.out.println(newLine + "Robot inverse kinematics:" + newLine + "q start" + newLine + vs.toString("%8.3f") + newLine + "q final" + newLine + puma.invKine(T).toString("%8.3f") + newLine + "q real" + newLine + vr.toString("%8.3f")); /* -------------- DYNAMICS --------------*/ // create robot from array data puma = new DHRobot(6, PUMA560_data); System.out.println(newLine + "=====================================================" + newLine + "Robot PUMA560 dynamics" + newLine + "=====================================================" + newLine); System.out.println("Robot joints variables" + newLine + puma.getQ().toString("%8.3f" + newLine)); System.out.println("Robot Inertia matrix" + newLine + puma.inertia(puma.getQ())); System.out.println("Robot torque" + newLine + puma.torque(puma.getQ(), new RVector(6), new RVector(6))); System.out.println("Robot acceleration" + newLine + puma .acceleration(puma.getQ(), new RVector(6), new RVector( 6))); /* -------------- MOTOR DYNAMICS --------------*/ // create robot from array data puma = new DHRobot(6, PUMA560_data0, PUMA560_motor); System.out.println(newLine + "=====================================================" + newLine + "Robot PUMA560 with motor dynamics" + newLine + "=====================================================" + newLine); System.out.println("Robot joints variables" + newLine + puma.getQ().toString("%8.3f" + newLine)); System.out.println("Robot Inertia matrix" + newLine + puma.inertia(puma.getQ())); System.out.println("Robot torque" + newLine + puma.torque(puma.getQ(), new RVector(6), new RVector(6))); System.out.println("Robot acceleration" + newLine + puma .acceleration(puma.getQ(), new RVector(6), new RVector( 6))); final long estimatedTime = System.nanoTime() - startTime; System.out.println("Estimated time:" + newLine + estimatedTime / 1e+9 + " [s]"); } }