Examples with RotationNR com.neuronrobotics.sdk.addons.kinematics.math.RotationNR used on opensource projects

Example 6 with RotationNR

use of com.neuronrobotics.sdk.addons.kinematics.math.RotationNR in project BowlerStudio by CommonWealthRobotics.

the class JogMobileBase method controllerLoop.

private void controllerLoop() {
    new Thread(() -> {
        // System.out.println("controllerLoop");
        double seconds = .1;
        if (stop == false) {
            try {
                seconds = Double.parseDouble(sec.getText());
                if (!stop) {
                    double inc;
                    try {
                        // convert to mm
                        inc = Double.parseDouble(increment.getText()) * 1000 * seconds;
                    } catch (Exception e) {
                        inc = defauletSpeed;
                        Platform.runLater(() -> {
                            try {
                                increment.setText(Double.toString(defauletSpeed));
                            } catch (Exception ex) {
                                ex.printStackTrace();
                            }
                        });
                    }
                    // double rxl=0;
                    double ryl = inc / 20 * slider;
                    double rzl = inc / 2 * rz;
                    TransformNR current = new TransformNR(0, 0, 0, new RotationNR(0, rzl, 0));
                    current.translateX(inc * x);
                    current.translateY(inc * y);
                    current.translateZ(inc * slider);
                    try {
                        TransformNR toSet = current.copy();
                        double toSeconds = seconds;
                        JogThread.setTarget(mobilebase, toSet, toSeconds);
                    } catch (Exception e) {
                        // TODO Auto-generated catch block
                        e.printStackTrace();
                    }
                }
            } catch (Exception e) {
                e.printStackTrace();
            }
            if (seconds < .01) {
                seconds = .01;
                Platform.runLater(() -> sec.setText(".01"));
            }
            FxTimer.runLater(Duration.ofMillis((int) (seconds * 1000.0)), new Runnable() {

                @Override
                public void run() {
                    controllerLoop();
                // System.out.println("Controller loop!");
                }
            });
        }
    }).start();
}

Example 7 with RotationNR

use of com.neuronrobotics.sdk.addons.kinematics.math.RotationNR in project java-bowler by NeuronRobotics.

the class AbstractKinematicsNR method loadConfig.

/**
 * Load XML configuration file,
 * then store in LinkConfiguration (ArrayList type)
 */
protected ArrayList<LinkConfiguration> loadConfig(Element doc) {
    ArrayList<LinkConfiguration> localConfigsFromXml = new ArrayList<LinkConfiguration>();
    NodeList nodListofLinks = doc.getChildNodes();
    setCadEngine(getGistCodes(doc, "cadEngine"));
    setDhEngine(getGistCodes(doc, "kinematics"));
    for (int i = 0; i < nodListofLinks.getLength(); i++) {
        Node linkNode = nodListofLinks.item(i);
        if (linkNode.getNodeType() == Node.ELEMENT_NODE && linkNode.getNodeName().contentEquals("link")) {
            localConfigsFromXml.add(new LinkConfiguration((Element) linkNode));
            NodeList dHParameters = linkNode.getChildNodes();
            for (int x = 0; x < dHParameters.getLength(); x++) {
                Node nNode = dHParameters.item(x);
                if (nNode.getNodeType() == Node.ELEMENT_NODE && nNode.getNodeName().contentEquals("DHParameters")) {
                    Element dhNode = (Element) nNode;
                    DHLink newLink = new DHLink(dhNode);
                    // 0->1
                    getDhParametersChain().addLink(newLink);
                    NodeList mobileBasesNodeList = dhNode.getChildNodes();
                    for (int j = 0; j < mobileBasesNodeList.getLength(); j++) {
                        Node mb = mobileBasesNodeList.item(j);
                        if (mb.getNodeType() == Node.ELEMENT_NODE && mb.getNodeName().contentEquals("mobilebase")) {
                            final MobileBase newMobileBase = new MobileBase((Element) mb);
                            mobileBases.add(newMobileBase);
                            newLink.setMobileBaseXml(newMobileBase);
                            newLink.addDhLinkPositionListener(new IDhLinkPositionListener() {

                                @Override
                                public void onLinkGlobalPositionChange(TransformNR newPose) {
                                    Log.debug("Motion in the D-H link has caused this mobile base to move");
                                    newMobileBase.setGlobalToFiducialTransform(newPose);
                                }
                            });
                        }
                    }
                }
            }
        } else if (linkNode.getNodeType() == Node.ELEMENT_NODE && linkNode.getNodeName().contentEquals("name")) {
            try {
                setScriptingName(XmlFactory.getTagValue("name", doc));
            } catch (Exception E) {
                E.printStackTrace();
            }
        } else if (linkNode.getNodeType() == Node.ELEMENT_NODE && linkNode.getNodeName().contentEquals("ZframeToRAS")) {
            Element eElement = (Element) linkNode;
            setZframeToGlobalTransform(new TransformNR(Double.parseDouble(XmlFactory.getTagValue("x", eElement)), Double.parseDouble(XmlFactory.getTagValue("y", eElement)), Double.parseDouble(XmlFactory.getTagValue("z", eElement)), new RotationNR(new double[] { Double.parseDouble(XmlFactory.getTagValue("rotw", eElement)), Double.parseDouble(XmlFactory.getTagValue("rotx", eElement)), Double.parseDouble(XmlFactory.getTagValue("roty", eElement)), Double.parseDouble(XmlFactory.getTagValue("rotz", eElement)) })));
        } else if (linkNode.getNodeType() == Node.ELEMENT_NODE && linkNode.getNodeName().contentEquals("baseToZframe")) {
            Element eElement = (Element) linkNode;
            setBaseToZframeTransform(new TransformNR(Double.parseDouble(XmlFactory.getTagValue("x", eElement)), Double.parseDouble(XmlFactory.getTagValue("y", eElement)), Double.parseDouble(XmlFactory.getTagValue("z", eElement)), new RotationNR(new double[] { Double.parseDouble(XmlFactory.getTagValue("rotw", eElement)), Double.parseDouble(XmlFactory.getTagValue("rotx", eElement)), Double.parseDouble(XmlFactory.getTagValue("roty", eElement)), Double.parseDouble(XmlFactory.getTagValue("rotz", eElement)) })));
        } else {
            // System.err.println(linkNode.getNodeName());
            Log.error("Node not known: " + linkNode.getNodeName());
        }
    }
    return localConfigsFromXml;
}

Example 8 with RotationNR

use of com.neuronrobotics.sdk.addons.kinematics.math.RotationNR in project java-bowler by NeuronRobotics.

the class ServoStockGCodeParser method addHandlers.

void addHandlers(GCodeInterpreter interp) {
    interp.setErrorHandler(new CodeHandler() {

        public void execute(GCodeLineData prev, GCodeLineData next) throws Exception {
            firePrinterStatusUpdate(new PrinterStatus(currentTransform, extrusion, currentTempreture, (int) next.getWord('P'), PrinterState.ERROR, next + " unhandled exception"));
        }
    });
    // Temperature control
    interp.addMHandler(104, new CodeHandler() {

        public void execute(GCodeLineData prev, GCodeLineData next) throws Exception {
            waitForClearToPrint();
            currentTempreture = next.getWord('S');
            device.setExtrusionTempreture(currentTempreture);
            currentLine = (int) next.getWord('P');
            firePrinterStatusUpdate(PrinterState.PRINTING);
        }
    });
    // TODO this code should wait until up to tempreture
    interp.addMHandler(109, new CodeHandler() {

        public void execute(GCodeLineData prev, GCodeLineData next) throws Exception {
            waitForClearToPrint();
            currentTempreture = next.getWord('S');
            device.setExtrusionTempreture(currentTempreture);
            currentLine = (int) next.getWord('P');
            firePrinterStatusUpdate(PrinterState.PRINTING);
        }
    });
    interp.setGHandler(0, new CodeHandler() {

        public void execute(GCodeLineData prev, GCodeLineData next) throws Exception {
            waitForClearToPrint();
            currentTransform = new TransformNR(next.getWord('X'), next.getWord('Y'), next.getWord('Z'), new RotationNR());
            extrusion = next.getWord('E');
            // zero seconds is a rapid
            device.setDesiredPrintLocetion(currentTransform, extrusion, 0);
            currentLine = (int) next.getWord('P');
            firePrinterStatusUpdate(PrinterState.PRINTING);
        }
    });
    interp.setGHandler(28, new CodeHandler() {

        // Move to origin
        public void execute(GCodeLineData prev, GCodeLineData next) throws Exception {
            waitForClearToPrint();
            currentTransform = new TransformNR(0, 0, 0, new RotationNR());
            // zero seconds is a rapid
            device.setDesiredPrintLocetion(currentTransform, extrusion, 0);
            currentLine = (int) next.getWord('P');
            firePrinterStatusUpdate(PrinterState.PRINTING);
        }
    });
    interp.setGHandler(92, new CodeHandler() {

        // Move to origin
        public void execute(GCodeLineData prev, GCodeLineData next) throws Exception {
            // clear the print queue before zeroing out extruder
            waitForEmptyPrintQueue();
            extrusion = next.getWord('E');
            device.zeroExtrusion(extrusion);
            currentLine = (int) next.getWord('P');
            firePrinterStatusUpdate(PrinterState.PRINTING);
        }
    });
    // set units to millimeters
    interp.setGHandler(21, new CodeHandler() {

        // Move to origin
        public void execute(GCodeLineData prev, GCodeLineData next) throws Exception {
            waitForClearToPrint();
            currentLine = (int) next.getWord('P');
            firePrinterStatusUpdate(PrinterState.PRINTING);
        }
    });
    // use absolute coordinates
    interp.setGHandler(90, new CodeHandler() {

        // Move to origin
        public void execute(GCodeLineData prev, GCodeLineData next) throws Exception {
            waitForClearToPrint();
            currentLine = (int) next.getWord('P');
            firePrinterStatusUpdate(PrinterState.PRINTING);
        }
    });
    interp.setGHandler(1, new CodeHandler() {

        public void execute(GCodeLineData prev, GCodeLineData next) throws Exception {
            waitForClearToPrint();
            currentTransform = new TransformNR(next.getWord('X'), next.getWord('Y'), next.getWord('Z'), new RotationNR());
            TransformNR prevT = new TransformNR(prev.getWord('X'), prev.getWord('Y'), prev.getWord('Z'), new RotationNR());
            double seconds = (currentTransform.getOffsetVectorMagnitude(prevT) / next.getWord('F')) * 60.0;
            extrusion = next.getWord('E');
            int iter = 0;
            while (iter++ < 1000) {
                try {
                    device.setDesiredPrintLocetion(currentTransform, extrusion, seconds);
                    currentLine = (int) next.getWord('P');
                    firePrinterStatusUpdate(PrinterState.PRINTING);
                    return;
                } catch (RuntimeException ex) {
                    // keep trying
                    Thread.sleep(100);
                }
            }
        }
    });
}

Example 9 with RotationNR

use of com.neuronrobotics.sdk.addons.kinematics.math.RotationNR in project bowler-script-kernel by CommonWealthRobotics.

the class TransformFactory method csgToNR.

public static TransformNR csgToNR(eu.mihosoft.vrl.v3d.Transform csg) {
    Matrix4d rotation = csg.getInternalMatrix();
    Quat4d q1 = new Quat4d();
    rotation.get(q1);
    Vector3d t1 = new Vector3d();
    rotation.get(t1);
    return new TransformNR(t1.x, t1.y, t1.z, new RotationNR(q1.w, q1.x, q1.y, q1.z));
}

Example 10 with RotationNR

use of com.neuronrobotics.sdk.addons.kinematics.math.RotationNR in project java-bowler by NeuronRobotics.

the class DeltaRobotKinematics method delta_calcForward.

// forward kinematics: (theta1, theta2, theta3) -> (x0, y0, z0)
// returned status:  CartesianCoordinante=OK, null=non-existing position
public TransformNR delta_calcForward(double[] input) {
    double x0, y0, z0;
    for (int i = 0; i < 3; i++) {
        if (input[i] >= 85)
            throw new RuntimeException("Angle hit toggle point: " + input[i]);
    }
    double theta1 = Math.toRadians(input[0]);
    double theta2 = Math.toRadians(input[1]);
    double theta3 = Math.toRadians(input[2]);
    double t = (getF() - getE()) * tan30 / 2;
    double y1 = -(t + getRf() * Math.cos(theta1));
    double z1 = -getRf() * Math.sin(theta1);
    double y2 = (t + getRf() * Math.cos(theta2)) * sin30;
    double x2 = y2 * tan60;
    double z2 = -getRf() * Math.sin(theta2);
    double y3 = (t + getRf() * Math.cos(theta3)) * sin30;
    double x3 = -y3 * tan60;
    double z3 = -getRf() * Math.sin(theta3);
    double dnm = (y2 - y1) * x3 - (y3 - y1) * x2;
    double w1 = y1 * y1 + z1 * z1;
    double w2 = x2 * x2 + y2 * y2 + z2 * z2;
    double w3 = x3 * x3 + y3 * y3 + z3 * z3;
    // x = (a1*z + b1)/dnm
    double a1 = (z2 - z1) * (y3 - y1) - (z3 - z1) * (y2 - y1);
    double b1 = -((w2 - w1) * (y3 - y1) - (w3 - w1) * (y2 - y1)) / 2.0;
    // y = (a2*z + b2)/dnm;
    double a2 = -(z2 - z1) * x3 + (z3 - z1) * x2;
    double b2 = ((w2 - w1) * x3 - (w3 - w1) * x2) / 2.0;
    // a*z^2 + b*z + c = 0
    double a = a1 * a1 + a2 * a2 + dnm * dnm;
    double b = 2 * (a1 * b1 + a2 * (b2 - y1 * dnm) - z1 * dnm * dnm);
    double c = (b2 - y1 * dnm) * (b2 - y1 * dnm) + b1 * b1 + dnm * dnm * (z1 * z1 - getRe() * getRe());
    // discriminant
    double d = b * b - (double) 4.0 * a * c;
    // non-existing point
    if (d < 0)
        return null;
    z0 = -(double) 0.5 * (b + Math.sqrt(d)) / a;
    x0 = (a1 * z0 + b1) / dnm;
    y0 = (a2 * z0 + b2) / dnm;
    return new TransformNR(x0, y0, z0, new RotationNR());
}