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

Example 16 with TransformNR

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

the class DHChain method getJacobian.

/**
 * Gets the Jacobian matrix
 *
 * @return a matrix representing the Jacobian for the current configuration
 */
public Matrix getJacobian(double[] jointSpaceVector) {
    double[][] data = new double[getLinks().size()][6];
    getChain(jointSpaceVector);
    for (int i = 0; i < getLinks().size(); i++) {
        double[] zVect = new double[3];
        if (i == 0) {
            zVect[0] = 0;
            zVect[1] = 0;
            zVect[2] = 1;
        } else {
            // Get the rz vector from matrix
            zVect[0] = intChain.get(i - 1).getRotationMatrix().getRotationMatrix()[0][2];
            zVect[1] = intChain.get(i - 1).getRotationMatrix().getRotationMatrix()[1][2];
            zVect[2] = intChain.get(i - 1).getRotationMatrix().getRotationMatrix()[2][2];
        }
        // Assume all rotational joints
        // Set to zero if prismatic
        data[i][3] = zVect[0];
        data[i][4] = zVect[1];
        data[i][5] = zVect[2];
        // Figure out the current
        Matrix current = new TransformNR().getMatrixTransform();
        for (int j = i; j < getLinks().size(); j++) {
            Matrix step = getLinks().get(j).DhStepRotory(Math.toRadians(jointSpaceVector[j]));
            // Log.info( "Current:\n"+current+"Step:\n"+step);
            current = current.times(step);
        }
        double[] rVect = new double[3];
        TransformNR tmp = new TransformNR(current);
        rVect[0] = tmp.getX();
        rVect[1] = tmp.getY();
        rVect[2] = tmp.getZ();
        // Cross product of rVect and Z vect
        double[] xProd = crossProduct(rVect, zVect);
        data[i][0] = xProd[0];
        data[i][1] = xProd[1];
        data[i][2] = xProd[2];
    }
    return new Matrix(data);
}

Example 17 with TransformNR

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

the class GradiantDecentNode method stepOrent.

public boolean stepOrent() {
    double none = myStart + offset;
    double start = offset;
    jointSpaceVector[getIndex()] = bound(none);
    TransformNR tmp = chain.forwardKinematics(jointSpaceVector);
    tmp = chain.forwardKinematics(jointSpaceVector);
    double noneOrent = tmp.getOffsetOrentationMagnitude(target);
    // Multiply by magic number
    double incOrentP = (noneOrent * 10);
    // Remove old values off rolling buffer
    integralTotalOrent -= intOrent[integralIndexOrent];
    // Store current values
    intOrent[integralIndexOrent] = incOrentP;
    // Add current values to totals
    integralTotalOrent += intOrent[integralIndexOrent];
    // Reset the index for next iteration
    integralIndexOrent++;
    if (integralIndexOrent == integralSize) {
        integralIndexOrent = 0;
    }
    // The 2 increment numbers
    incOrent = incOrentP * Kp + (integralTotalOrent / integralSize) * Ki;
    double upO = myStart + offset + incOrent;
    double downO = myStart + offset - incOrent;
    jointSpaceVector[getIndex()] = bound(upO);
    tmp = chain.forwardKinematics(jointSpaceVector);
    double upOrent = tmp.getOffsetOrentationMagnitude(target);
    jointSpaceVector[getIndex()] = bound(downO);
    tmp = chain.forwardKinematics(jointSpaceVector);
    double downOrent = tmp.getOffsetOrentationMagnitude(target);
    if ((upOrent > noneOrent && downOrent > noneOrent)) {
        jointSpaceVector[getIndex()] = none;
    }
    if ((noneOrent > upOrent && downOrent > upOrent)) {
        jointSpaceVector[getIndex()] = upO;
        offset += incOrent;
    }
    if ((upOrent > downOrent && noneOrent > downOrent)) {
        jointSpaceVector[getIndex()] = downO;
        offset -= incOrent;
    }
    jointSpaceVector[getIndex()] = myStart + offset;
    if (start == offset)
        return true;
    return false;
}

Example 18 with TransformNR

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

the class GradiantDecentNode method stepLin.

public boolean stepLin() {
    double none = myStart + offset;
    double start = offset;
    jointSpaceVector[getIndex()] = bound(none);
    TransformNR tmp = chain.forwardKinematics(jointSpaceVector);
    tmp = chain.forwardKinematics(jointSpaceVector);
    double nonevect = tmp.getOffsetVectorMagnitude(target);
    // Divide by magic number
    double incVectP = (nonevect / 1000);
    // Remove old values off rolling buffer
    integralTotalVect -= intVect[integralIndexVect];
    // Store current values
    intVect[integralIndexVect] = incVectP;
    // Add current values to totals
    integralTotalVect += intVect[integralIndexVect];
    // Reset the index for next iteration
    integralIndexVect++;
    if (integralIndexVect == integralSize) {
        integralIndexVect = 0;
    }
    // The 2 increment numbers
    incVect = incVectP * Kp + (integralTotalVect / integralSize) * Ki;
    double up = myStart + offset + incVect;
    double down = myStart + offset - incVect;
    jointSpaceVector[getIndex()] = bound(up);
    tmp = chain.forwardKinematics(jointSpaceVector);
    double upvect = tmp.getOffsetVectorMagnitude(target);
    jointSpaceVector[getIndex()] = bound(down);
    tmp = chain.forwardKinematics(jointSpaceVector);
    double downvect = tmp.getOffsetVectorMagnitude(target);
    if ((upvect > nonevect && downvect > nonevect)) {
        jointSpaceVector[getIndex()] = none;
    }
    if ((nonevect > upvect && downvect > upvect)) {
        jointSpaceVector[getIndex()] = up;
        offset += incVect;
    }
    if ((upvect > downvect && nonevect > downvect)) {
        jointSpaceVector[getIndex()] = down;
        offset -= incVect;
    }
    jointSpaceVector[getIndex()] = myStart + offset;
    if (start == offset)
        return true;
    return false;
}

Example 19 with TransformNR

use of com.neuronrobotics.sdk.addons.kinematics.math.TransformNR 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 20 with TransformNR

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

the class NRPrinter method printStatus.

@Override
public void printStatus(PrinterStatus psl) {
    // TODO Auto-generated method stub
    if (psl.getDriverState() == PrinterState.MOVING)
        firePoseTransform(forwardOffset(psl.getHeadLocation()));
    if (psl.getDriverState() == PrinterState.PRINTING) {
        // Log.warning("Received a Print status update");
        TransformNR taskSpaceTransform = psl.getHeadLocation();
        fireTargetJointsUpdate(getCurrentJointSpaceVector(), taskSpaceTransform);
    }
}