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

Example 11 with RotationNR

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

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

the class MobileBasePhysicsManager method getUpdater.

private IPhysicsUpdate getUpdater(RigidBody body, IMU base) {
    return new IPhysicsUpdate() {

        Vector3f oldavelocity = new Vector3f(0f, 0f, 0f);

        Vector3f oldvelocity = new Vector3f(0f, 0f, 0f);

        Vector3f gravity = new Vector3f();

        private Quat4f orentation = new Quat4f();

        Transform gravTrans = new Transform();

        Transform orentTrans = new Transform();

        Vector3f avelocity = new Vector3f();

        Vector3f velocity = new Vector3f();

        @Override
        public void update(float timeStep) {
            body.getAngularVelocity(avelocity);
            body.getLinearVelocity(velocity);
            body.getGravity(gravity);
            body.getOrientation(orentation);
            TransformFactory.nrToBullet(new TransformNR(gravity.x, gravity.y, gravity.z, new RotationNR()), gravTrans);
            TransformFactory.nrToBullet(new TransformNR(0, 0, 0, orentation.w, orentation.x, orentation.y, orentation.z), orentTrans);
            orentTrans.inverse();
            orentTrans.mul(gravTrans);
            // A=DeltaV / DeltaT
            Double rotxAcceleration = (double) ((oldavelocity.x - avelocity.x) / timeStep);
            Double rotyAcceleration = (double) ((oldavelocity.y - avelocity.y) / timeStep);
            Double rotzAcceleration = (double) ((oldavelocity.z - avelocity.z) / timeStep);
            Double xAcceleration = (double) (((oldvelocity.x - velocity.x) / timeStep) / PhysicsGravityScalar) + (orentTrans.origin.x / PhysicsGravityScalar);
            Double yAcceleration = (double) (((oldvelocity.y - velocity.y) / timeStep) / PhysicsGravityScalar) + (orentTrans.origin.y / PhysicsGravityScalar);
            Double zAcceleration = (double) (((oldvelocity.z - velocity.z) / timeStep) / PhysicsGravityScalar) + (orentTrans.origin.z / PhysicsGravityScalar);
            // tell the virtual IMU the system updated
            base.setVirtualState(new IMUUpdate(xAcceleration, yAcceleration, zAcceleration, rotxAcceleration, rotyAcceleration, rotzAcceleration));
            // update the old variables
            oldavelocity.set(avelocity);
            oldvelocity.set(velocity);
        }
    };
}

Example 13 with RotationNR

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

the class WalkingDriveEngine method DriveArc.

@Override
public void DriveArc(MobileBase source, TransformNR newPose, double seconds) {
    int numlegs = source.getLegs().size();
    TransformNR[] feetLocations = new TransformNR[numlegs];
    TransformNR[] home = new TransformNR[numlegs];
    ArrayList<DHParameterKinematics> legs = source.getLegs();
    // Load in the locations of the tips of each of the feet.
    for (int i = 0; i < numlegs; i++) {
        feetLocations[i] = legs.get(i).getCurrentPoseTarget();
        home[i] = legs.get(i).calcHome();
    }
    // Apply transform to each dimention of current pose
    TransformNR global = source.getFiducialToGlobalTransform();
    global.translateX(newPose.getX());
    global.translateY(newPose.getY());
    global.translateZ(newPose.getZ());
    double rotz = newPose.getRotation().getRotationZ() + global.getRotation().getRotationZ();
    double roty = newPose.getRotation().getRotationY();
    double rotx = newPose.getRotation().getRotationX();
    global.setRotation(new RotationNR(rotx, roty, rotz));
    // New target calculated appliaed to global offset
    source.setGlobalToFiducialTransform(global);
    for (int i = 0; i < numlegs; i++) {
        if (!legs.get(i).checkTaskSpaceTransform(feetLocations[i])) {
            // new leg position is not reachable, reverse course and walk up the line to a better location
            do {
                feetLocations[i].translateX(-newPose.getX());
                feetLocations[i].translateY(-newPose.getY());
            } while (legs.get(i).checkTaskSpaceTransform(feetLocations[i]));
            // step back one increment for new location
            feetLocations[i].translateX(newPose.getX());
            feetLocations[i].translateY(newPose.getY());
            // perform the step over
            home[i].translateZ(stepOverHeight);
            try {
                // lift leg above home
                legs.get(i).setDesiredTaskSpaceTransform(home[i], seconds / 10);
                ThreadUtil.wait((int) (seconds * 100));
                // step to new target
                legs.get(i).setDesiredTaskSpaceTransform(feetLocations[i], seconds / 10);
                ThreadUtil.wait((int) (seconds * 100));
                // set new target for the coordinated motion step at the end
                feetLocations[i].translateX(newPose.getX());
                feetLocations[i].translateY(newPose.getY());
            } catch (Exception e) {
                // TODO Auto-generated catch block
                e.printStackTrace();
            }
        }
    }
    // all legs have a valid target set, perform coordinated motion
    for (int i = 0; i < numlegs; i++) {
        try {
            legs.get(i).setDesiredTaskSpaceTransform(feetLocations[i], seconds);
        } catch (Exception e) {
            // TODO Auto-generated catch block
            e.printStackTrace();
        }
    }
}

Example 14 with RotationNR

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

the class RotationNRTest method test.

@Test
public void test() {
    int failCount = 0;
    for (int i = 0; i < 100; i++) {
        double tilt = Math.toRadians(Math.random() * 360.0 - 180);
        double elevation = Math.toRadians(Math.random() * 360.0 - 180);
        double azumus = Math.toRadians(Math.random() * 360.0 - 180);
        RotationNR rotTest = new RotationNR(Math.toDegrees(tilt), Math.toDegrees(elevation), Math.toDegrees(azumus));
        System.out.println("\n\nTest #" + i);
        System.out.println("Testing Az=" + Math.toDegrees(azumus) + " El=" + Math.toDegrees(elevation) + " Tl=" + Math.toDegrees(tilt));
        System.out.println("Got Az=" + Math.toDegrees(rotTest.getRotationAzimuth()) + " El=" + Math.toDegrees(rotTest.getRotationElevation()) + " Tl=" + Math.toDegrees(rotTest.getRotationTilt()));
        if (!RotationNR.bound(tilt - .001, tilt + .001, rotTest.getRotationTilt())) {
            failCount++;
            System.err.println("Rotation Tilt is not consistant. expected " + Math.toDegrees(tilt) + " got " + Math.toDegrees(rotTest.getRotationTilt()));
        }
        if (!RotationNR.bound(elevation - .001, elevation + .001, rotTest.getRotationElevation())) {
            failCount++;
            System.err.println("Rotation Elevation is not consistant. expected " + Math.toDegrees(elevation) + " got " + Math.toDegrees(rotTest.getRotationElevation()));
        }
        if (!RotationNR.bound(azumus - .001, azumus + .001, rotTest.getRotationAzimuth())) {
            failCount++;
            System.err.println("Rotation Tilt is not consistant. expected " + Math.toDegrees(azumus) + " got " + Math.toDegrees(rotTest.getRotationAzimuth()));
        }
    }
    if (failCount > 200) {
        fail("Rotation failed " + failCount + " times");
    }
}

Example 15 with RotationNR

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

the class BowlerStudio3dEngine method focusInterpolate.

private void focusInterpolate(TransformNR start, TransformNR target, int depth, int targetDepth, Affine interpolator) {
    double depthScale = 1 - (double) depth / (double) targetDepth;
    double sinunsoidalScale = Math.sin(depthScale * (Math.PI / 2));
    // double xIncrement =target.getX()- ((start.getX() - target.getX()) *
    // depthScale) + start.getX();
    double difference = start.getX() - target.getX();
    double scaledDifference = (difference * sinunsoidalScale);
    double xIncrement = scaledDifference;
    double yIncrement = ((start.getY() - target.getY()) * sinunsoidalScale);
    double zIncrement = ((start.getZ() - target.getZ()) * sinunsoidalScale);
    Platform.runLater(() -> {
        interpolator.setTx(xIncrement);
        interpolator.setTy(yIncrement);
        interpolator.setTz(zIncrement);
    });
    // + " y " + yIncrement + " z " + zIncrement);
    if (depth < targetDepth) {
        FxTimer.runLater(Duration.ofMillis(16), new Runnable() {

            @Override
            public void run() {
                focusInterpolate(start, target, depth + 1, targetDepth, interpolator);
            }
        });
    } else {
        // System.err.println("Camera intrpolation done");
        Platform.runLater(() -> {
            focusGroup.getTransforms().remove(interpolator);
        });
        perviousTarget = target.copy();
        perviousTarget.setRotation(new RotationNR());
        focusing = false;
    }
}