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

Example 31 with TransformNR

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

the class ComputedGeometricModel method inverseKinematics.

public double[] inverseKinematics(TransformNR target, double[] jointSpaceVector, DHChain chain) {
    ArrayList<DHLink> links = chain.getLinks();
    // viewer.addTransform(target, "Target",Color.pink);
    int linkNum = jointSpaceVector.length;
    double[] inv = new double[linkNum];
    // Attempting to implement:
    // http://www.ri.cmu.edu/pub_files/pub1/xu_yangsheng_1993_1/xu_yangsheng_1993_1.pdf
    TransformNR current = dhChain.forwardKinematics(jointSpaceVector);
    return inv;
}

Example 32 with TransformNR

use of com.neuronrobotics.sdk.addons.kinematics.math.TransformNR 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());
}

Example 33 with TransformNR

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

the class DHChain method forwardKinematicsMatrix.

public Matrix forwardKinematicsMatrix(double[] jointSpaceVector, boolean store) {
    if (getLinks() == null)
        return new TransformNR().getMatrixTransform();
    if (jointSpaceVector.length != getLinks().size())
        throw new IndexOutOfBoundsException("DH links do not match defined links");
    Matrix current = new TransformNR().getMatrixTransform();
    if (store)
        setChain(new ArrayList<TransformNR>());
    for (int i = 0; i < getLinks().size(); i++) {
        LinkConfiguration conf = getFactory().getLinkConfigurations().get(i);
        Matrix step;
        if (conf.getType().isPrismatic())
            step = getLinks().get(i).DhStepPrismatic(jointSpaceVector[i]);
        else
            step = getLinks().get(i).DhStepRotory(Math.toRadians(jointSpaceVector[i]));
        // Log.info( "Current:\n"+current+"Step:\n"+step);
        current = current.times(step);
        final Matrix update = current.copy();
        final int index = i;
        final TransformNR pose = forwardOffset(new TransformNR(update));
        if (store) {
            if (intChain.size() <= i)
                intChain.add(new TransformNR(step));
            else {
                intChain.set(i, new TransformNR(step));
            }
            if (chain.size() <= i)
                chain.add(pose);
            else {
                chain.set(i, pose);
            }
        }
    }
    // Log.info( "Final:\n"+current);
    return current;
}

Example 34 with TransformNR

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

the class GenericKinematicsModelNR method forwardKinematics.

@Override
public TransformNR forwardKinematics(double[] jointSpaceVector) {
    double x = jointSpaceVector[0];
    double y = jointSpaceVector[1];
    double z = jointSpaceVector[2];
    Matrix rotX = new Matrix(RotationNR.getRotationX(jointSpaceVector[3]).getRotationMatrix());
    Matrix rotY = new Matrix(RotationNR.getRotationY(jointSpaceVector[4]).getRotationMatrix());
    Matrix rotZ = new Matrix(RotationNR.getRotationZ(jointSpaceVector[5]).getRotationMatrix());
    Matrix rotAll = rotX.times(rotY).times(rotZ);
    TransformNR back = new TransformNR(x, y, z, new RotationNR(rotAll));
    return back;
}

Example 35 with TransformNR

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

the class CSGPhysicsManager method loadCSGToPoints.

protected CSG loadCSGToPoints(CSG baseCSG, boolean adjustCenter, Transform pose, ObjectArrayList<Vector3f> arg0) {
    CSG finalCSG = baseCSG;
    if (adjustCenter) {
        double xcenter = baseCSG.getMaxX() / 2 + baseCSG.getMinX() / 2;
        double ycenter = baseCSG.getMaxY() / 2 + baseCSG.getMinY() / 2;
        double zcenter = baseCSG.getMaxZ() / 2 + baseCSG.getMinZ() / 2;
        TransformNR poseToMove = TransformFactory.bulletToNr(pose);
        if (baseCSG.getMaxX() < 1 || baseCSG.getMinX() > -1) {
            finalCSG = finalCSG.movex(-xcenter);
            poseToMove.translateX(xcenter);
        }
        if (baseCSG.getMaxY() < 1 || baseCSG.getMinY() > -1) {
            finalCSG = finalCSG.movey(-ycenter);
            poseToMove.translateY(ycenter);
        }
        if (baseCSG.getMaxZ() < 1 || baseCSG.getMinZ() > -1) {
            finalCSG = finalCSG.movez(-zcenter);
            poseToMove.translateZ(zcenter);
        }
        TransformFactory.nrToBullet(poseToMove, pose);
    }
    List<Polygon> polygons = finalCSG.getPolygons();
    // polygons = getBoundingBox(finalCSG).getPolygons();
    for (Polygon p : polygons) {
        for (Vertex v : p.vertices) {
            arg0.add(new Vector3f((float) v.getX(), (float) v.getY(), (float) v.getZ()));
        }
    }
    return finalCSG;
}