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

Example 1 with RotationNR

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

the class BowlerStudio3dEngine method handleMouse.

/**
 * Handle mouse.
 *
 * @param scene the scene
 */
private void handleMouse(SubScene scene) {
    scene.setOnMouseClicked(new EventHandler<MouseEvent>() {

        long lastClickedTimeLocal = 0;

        long offset = 500;

        @Override
        public void handle(MouseEvent event) {
            resetMouseTime();
            long lastClickedDifference = (System.currentTimeMillis() - lastClickedTimeLocal);
            FxTimer.runLater(Duration.ofMillis(100), new Runnable() {

                @Override
                public void run() {
                    long differenceIntime = System.currentTimeMillis() - lastSelectedTime;
                    if (differenceIntime > 2000) {
                        // reset only if an object is not being selected
                        if (lastClickedDifference < offset) {
                            cancelSelection();
                        // System.err.println("Cancel event detected");
                        }
                    } else {
                    // System.err.println("too soon after a select
                    // "+differenceIntime+" from "+lastSelectedTime);
                    }
                }
            });
            lastClickedTimeLocal = System.currentTimeMillis();
        }
    });
    scene.setOnMousePressed(new EventHandler<MouseEvent>() {

        @Override
        public void handle(MouseEvent me) {
            mousePosX = me.getSceneX();
            mousePosY = me.getSceneY();
            mouseOldX = me.getSceneX();
            mouseOldY = me.getSceneY();
            if (me.isPrimaryButtonDown())
                captureMouse = true;
            else
                captureMouse = false;
            resetMouseTime();
        }
    });
    scene.setOnMouseDragged(new EventHandler<MouseEvent>() {

        @Override
        public void handle(MouseEvent me) {
            resetMouseTime();
            mouseOldX = mousePosX;
            mouseOldY = mousePosY;
            mousePosX = me.getSceneX();
            mousePosY = me.getSceneY();
            mouseDeltaX = (mousePosX - mouseOldX);
            mouseDeltaY = (mousePosY - mouseOldY);
            double modifier = 1.0;
            double modifierFactor = 0.1;
            if (me.isControlDown()) {
                modifier = 0.1;
            }
            if (me.isShiftDown()) {
                modifier = 10.0;
            }
            if (me.isPrimaryButtonDown()) {
                // cameraXform.ry.setAngle(cameraXform.ry.getAngle()
                // - mouseDeltaX * modifierFactor * modifier * 2.0); // +
                // cameraXform.rx.setAngle(cameraXform.rx.getAngle()
                // + mouseDeltaY * modifierFactor * modifier * 2.0); // -
                // RotationNR roz = RotationNR.getRotationZ(-mouseDeltaX *
                // modifierFactor * modifier * 2.0);
                // RotationNR roy = RotationNR.getRotationY(mouseDeltaY *
                // modifierFactor * modifier * 2.);
                TransformNR trans = new TransformNR(0, 0, 0, new RotationNR(mouseDeltaY * modifierFactor * modifier * 2.0, mouseDeltaX * modifierFactor * modifier * 2.0, 0));
                if (me.isPrimaryButtonDown()) {
                    moveCamera(trans);
                }
            } else if (me.isMiddleButtonDown()) {
            } else if (me.isSecondaryButtonDown()) {
                double depth = -100 / getVirtualcam().getZoomDepth();
                moveCamera(new TransformNR(mouseDeltaX * modifierFactor * modifier * 1 / depth, mouseDeltaY * modifierFactor * modifier * 1 / depth, 0, new RotationNR()));
            }
        }
    });
    scene.addEventHandler(ScrollEvent.ANY, new EventHandler<ScrollEvent>() {

        @Override
        public void handle(ScrollEvent t) {
            if (ScrollEvent.SCROLL == t.getEventType()) {
                double zoomFactor = -(t.getDeltaY()) * getVirtualcam().getZoomDepth() / 500;
                // 
                // double z = camera.getTranslateY();
                // double newZ = z + zoomFactor;
                // camera.setTranslateY(newZ);
                // System.out.println("Z = "+zoomFactor);
                getVirtualcam().setZoomDepth(getVirtualcam().getZoomDepth() + zoomFactor);
            }
            t.consume();
        }
    });
}

Example 2 with RotationNR

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

the class VirtualCameraMobileBase method DriveArc.

public void DriveArc(TransformNR newPose) {
    // TODO Auto-generated method stub
    pureTrans.setX(newPose.getX());
    pureTrans.setY(newPose.getY());
    pureTrans.setZ(newPose.getZ());
    TransformNR global = getFiducialToGlobalTransform().times(pureTrans);
    global.setRotation(new RotationNR(tlOffset + (Math.toDegrees(newPose.getRotation().getRotationTilt() + global.getRotation().getRotationTilt()) % 360), azOffset + (Math.toDegrees(newPose.getRotation().getRotationAzimuth() + global.getRotation().getRotationAzimuth()) % 360), elOffset + Math.toDegrees(newPose.getRotation().getRotationElevation() + global.getRotation().getRotationElevation())));
    // global.getRotation().setStorage(nr);
    // System.err.println("Camera tilt="+global);
    // New target calculated appliaed to global offset
    setGlobalToFiducialTransform(global);
}

Example 3 with RotationNR

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

the class JogWidget method controllerLoop.

private void controllerLoop() {
    new Thread(() -> {
        // System.out.println("controllerLoop");
        double seconds = .1;
        if (getGameController() != null || 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 {
                        current = getKin().getCurrentPoseTarget().copy();
                        current.translateX(inc * x);
                        current.translateY(inc * y);
                        current.translateZ(inc * slider);
                        // current.setRotation(new RotationNR());
                        double toSeconds = seconds;
                        if (!JogThread.setTarget(getKin(), current, toSeconds)) {
                            current.translateX(-inc * x);
                            current.translateY(-inc * y);
                            current.translateZ(-inc * slider);
                        }
                    // Log.enableDebugPrint();
                    // System.out.println("Loop Jogging to: "+toSet);
                    } catch (Exception e) {
                        // TODO Auto-generated catch block
                        e.printStackTrace();
                    }
                }
            } catch (Exception e) {
                e.printStackTrace();
            }
            if (seconds < .01) {
                seconds = .01;
                sec.setText(".01");
            }
            FxTimer.runLater(Duration.ofMillis((int) (seconds * 1000.0)), new Runnable() {

                @Override
                public void run() {
                    controllerLoop();
                }
            });
        }
    }).start();
}

Example 4 with RotationNR

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

the class LinkSliderWidget method enable.

public void enable() {
    if (linkGaugeController3d == null) {
        linkGaugeController3d = new LinkGaugeController();
        BowlerStudioController.addUserNode(linkGaugeController3d.getGauge());
        offsetGauge = new Affine();
        offsetGaugeTranslate = new Affine();
        linkGaugeController3d.setSIZE(60);
    }
    double d = (((double) linkGaugeController3d.getSIZE())) / 2.0;
    TransformNR offsetter2 = new TransformNR().translateX(-d).translateY(-d);
    Platform.runLater(() -> TransformFactory.nrToAffine(offsetter2, offsetGaugeTranslate));
    TransformNR offsetter = new TransformNR();
    double theta = Math.toDegrees(device.getDhParametersChain().getLinks().get(linkIndex).getTheta());
    offsetter.setRotation(new RotationNR(0, 90 + theta, 0));
    Platform.runLater(() -> TransformFactory.nrToAffine(offsetter, offsetGauge));
    linkGaugeController3d.getGauge().getTransforms().clear();
    linkGaugeController3d.setLink(conf, getAbstractLink());
    if (linkIndex == 0)
        linkGaugeController3d.getGauge().getTransforms().add((Affine) device.getRootListener());
    else
        linkGaugeController3d.getGauge().getTransforms().add((Affine) device.getAbstractLink(linkIndex - 1).getGlobalPositionListener());
    linkGaugeController3d.getGauge().getTransforms().add(offsetGauge);
    linkGaugeController3d.getGauge().getTransforms().add(offsetGaugeTranslate);
}

Example 5 with RotationNR

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

the class TransformWidget method updatePose.

public void updatePose(TransformNR p) {
    TransformNR pose = p;
    Platform.runLater(() -> tx.setValue(pose.getX()));
    Platform.runLater(() -> ty.setValue(pose.getY()));
    Platform.runLater(() -> tz.setValue(pose.getZ()));
    RotationNR rot = pose.getRotation();
    double t = 0;
    try {
        t = Math.toDegrees(rot.getRotationTilt());
    } catch (Exception ex) {
        ex.printStackTrace();
    }
    double e = 0;
    try {
        e = Math.toDegrees(rot.getRotationElevation());
    } catch (Exception ex) {
        ex.printStackTrace();
    }
    double a = 0;
    try {
        a = Math.toDegrees(rot.getRotationAzimuth());
    } catch (Exception ex) {
        ex.printStackTrace();
    }
    double tiltVar = t;
    double eVar = e;
    double aVar = a;
    Platform.runLater(() -> tilt.setValue(tiltVar));
    Platform.runLater(() -> elevation.setValue(eVar));
    Platform.runLater(() -> azimeth.setValue(aVar));
    // Set the rotation after setting the UI so the read will load the rotation in its pure form
    initialState = p;
}