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

Example 41 with TransformNR

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

the class ParallelWidget method configure.

public void configure(MobileBase b, DHParameterKinematics dh, CreatureLab creatureLab) {
    resetting = true;
    this.base = b;
    this.dh = dh;
    this.creatureLab = creatureLab;
    System.out.println("Configuring arm " + dh.getScriptingName());
    robotToFiducialTransform = new TransformNR();
    Platform.runLater(() -> groupName.setText(""));
    Platform.runLater(() -> relativeName.getItems().clear());
    Platform.runLater(() -> relIndex.getItems().clear());
    Platform.runLater(() -> relIndex.setDisable(true));
    Platform.runLater(() -> e.setDisable(true));
    if (getGroup() == null) {
        useParallel.setSelected(false);
        box.setDisable(true);
    } else {
        useParallel.setSelected(true);
        box.setDisable(false);
        Platform.runLater(() -> groupName.setText(getGroup().getNameOfParallelGroup()));
        for (DHParameterKinematics l : base.getAllDHChains()) {
            if (!l.getScriptingName().contentEquals(dh.getScriptingName())) {
                System.out.println("Adding Option " + l.getScriptingName());
                Platform.runLater(() -> relativeName.getItems().add(l.getScriptingName()));
            }
        }
        if (getGroup().getTipOffset(dh) != null) {
            Platform.runLater(() -> useRelative.setSelected(true));
            Platform.runLater(() -> relativeToControls.setDisable(false));
            Platform.runLater(() -> relIndex.setDisable(false));
            Platform.runLater(() -> e.setDisable(false));
            robotToFiducialTransform = getGroup().getTipOffset(dh);
            Platform.runLater(() -> e.updatePose(robotToFiducialTransform));
            String refLimbName = getGroup().getTipOffsetRelativeName(dh);
            setNewReferencedLimb(base, refLimbName);
            Platform.runLater(() -> relativeName.setValue(refLimbName));
            Platform.runLater(() -> relIndex.setValue(getGroup().getTipOffsetRelativeIndex(dh)));
        } else {
            Platform.runLater(() -> useRelative.setSelected(false));
            Platform.runLater(() -> relativeToControls.setDisable(true));
        }
    }
    e.updatePose(robotToFiducialTransform);
    Platform.runLater(() -> resetting = false);
    home();
}

Example 42 with TransformNR

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

the class BowlerStudio3dEngine method cancelSelection.

public void cancelSelection() {
    for (CSG key : getCsgMap().keySet()) {
        Platform.runLater(() -> getCsgMap().get(key).setMaterial(new PhongMaterial(key.getColor())));
    }
    this.selectedCsg = null;
    // new Exception().printStackTrace();
    TransformNR startSelectNr = perviousTarget.copy();
    TransformNR targetNR = new TransformNR();
    Affine interpolator = new Affine();
    Platform.runLater(() -> {
        TransformFactory.nrToAffine(startSelectNr, interpolator);
        removeAllFocusTransforms();
        focusGroup.getTransforms().add(interpolator);
        focusInterpolate(startSelectNr, targetNR, 0, 15, interpolator);
    });
    resetMouseTime();
}

Example 43 with TransformNR

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

the class BowlerStudio3dEngine method focusToAffine.

private void focusToAffine(TransformNR poseToMove, Affine manipulator2) {
    if (focusing)
        return;
    if (manipulator2 == null) {
        new RuntimeException("Can not focus on null affine").printStackTrace();
        return;
    }
    focusing = true;
    Platform.runLater(() -> {
        Affine centering = TransformFactory.nrToAffine(poseToMove);
        // this section keeps the camera orented the same way to avoid whipping
        // around
        TransformNR rotationOnlyCOmponentOfManipulator = TransformFactory.affineToNr(manipulator2);
        rotationOnlyCOmponentOfManipulator.setX(0);
        rotationOnlyCOmponentOfManipulator.setY(0);
        rotationOnlyCOmponentOfManipulator.setZ(0);
        TransformNR reverseRotation = rotationOnlyCOmponentOfManipulator.inverse();
        TransformNR startSelectNr = perviousTarget.copy();
        // =
        TransformNR targetNR;
        // TransformFactory.affineToNr(selectedCsg.getManipulator());
        if (Math.abs(manipulator2.getTx()) > 0.1 || Math.abs(manipulator2.getTy()) > 0.1 || Math.abs(manipulator2.getTz()) > 0.1) {
            targetNR = TransformFactory.affineToNr(manipulator2);
        } else {
            targetNR = TransformFactory.affineToNr(centering);
        }
        Affine interpolator = new Affine();
        Affine correction = TransformFactory.nrToAffine(reverseRotation);
        interpolator.setTx(startSelectNr.getX() - targetNR.getX());
        interpolator.setTy(startSelectNr.getY() - targetNR.getY());
        interpolator.setTz(startSelectNr.getZ() - targetNR.getZ());
        removeAllFocusTransforms();
        focusGroup.getTransforms().add(interpolator);
        try {
            if (Math.abs(manipulator2.getTx()) > 0.1 || Math.abs(manipulator2.getTy()) > 0.1 || Math.abs(manipulator2.getTz()) > 0.1) {
                // Platform.runLater(() -> {
                focusGroup.getTransforms().add(manipulator2);
                focusGroup.getTransforms().add(correction);
            // });
            } else
                // Platform.runLater(() -> {
                focusGroup.getTransforms().add(centering);
        // });
        } catch (Exception ex) {
        }
        focusInterpolate(startSelectNr, targetNR, 0, 30, interpolator);
    });
}