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

Example 36 with TransformNR

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

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

the class TransformFactory method bulletToNr.

public static TransformNR bulletToNr(com.bulletphysics.linearmath.Transform bullet) {
    Quat4f out = new Quat4f();
    bullet.getRotation(out);
    return new TransformNR(bullet.origin.x, bullet.origin.y, bullet.origin.z, out.w, out.x, out.y, out.z);
}

Example 38 with TransformNR

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

the class WheelCSGPhysicsManager method update.

@Override
public void update(float timeStep) {
    // cut out the falling body update
    if (getUpdateManager() != null) {
        try {
            getUpdateManager().update(timeStep);
        } catch (Exception e) {
            // BowlerStudio.printStackTrace(e);
            throw e;
        }
    }
    if (getController() != null) {
        velocity = getController().compute(getWheelInfo().rotation, getTarget(), timeStep);
    }
    vehicle.updateWheelTransform(getWheelIndex(), true);
    TransformNR trans = TransformFactory.bulletToNr(vehicle.getWheelInfo(getWheelIndex()).worldTransform);
    // copy in the current wheel location
    TransformFactory.nrToBullet(trans, getUpdateTransform());
}

Example 39 with TransformNR

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

the class PosePanelNR method setPose.

public void setPose() {
    if (!input) {
        double t = Double.parseDouble(time.getText());
        TransformNR target = new TransformNR(matrix.getTableDataMatrix());
        try {
            Log.info("GUI Seting pose :" + target);
            getModel().setDesiredTaskSpaceTransform(target, t);
        } catch (Exception e1) {
            e1.printStackTrace();
            JOptionPane.showMessageDialog(new JFrame(), e1.getMessage(), "Range warning", JOptionPane.WARNING_MESSAGE);
        }
    }
}

Example 40 with TransformNR

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