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

Example 26 with TransformNR

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

the class AbstractKinematicsNR method getCurrentTaskSpaceTransform.

/**
 * This takes a reading of the robots position and converts it to a joint space vector
 * This vector is converted to task space and returned
 * @return taskSpaceVector in mm,radians [x,y,z,rotx,rotY,rotZ]
 */
public TransformNR getCurrentTaskSpaceTransform() {
    TransformNR fwd = forwardKinematics(getCurrentJointSpaceVector());
    if (fwd == null)
        throw new RuntimeException("Implementations of the kinematics need to return a transform not null");
    // Log.info("Getting robot task space "+fwd);
    TransformNR taskSpaceTransform = forwardOffset(fwd);
    // Log.info("Getting global task space "+taskSpaceTransform);
    return taskSpaceTransform;
}

Example 27 with TransformNR

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

the class AbstractKinematicsNR method inverseOffset.

protected TransformNR inverseOffset(TransformNR t) {
    // System.out.println("RobotToFiducialTransform "+getRobotToFiducialTransform());
    // System.out.println("FiducialToRASTransform "+getFiducialToRASTransform());
    Matrix rtz = getFiducialToGlobalTransform().getMatrixTransform().inverse();
    Matrix ztr = getRobotToFiducialTransform().getMatrixTransform().inverse();
    Matrix current = t.getMatrixTransform();
    Matrix mForward = ztr.times(rtz).times(current);
    return new TransformNR(mForward);
}

Example 28 with TransformNR

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

the class AbstractKinematicsNR method setBaseToZframeTransform.

public void setBaseToZframeTransform(TransformNR baseToFiducial) {
    Log.info("Setting Fiducial To base Transform " + baseToFiducial);
    this.base2Fiducial = baseToFiducial;
    for (IRegistrationListenerNR r : regListeners) {
        r.onBaseToFiducialUpdate(this, baseToFiducial);
    }
    Platform.runLater(new Runnable() {

        @Override
        public void run() {
            TransformFactory.getTransform(forwardOffset(new TransformNR()), root);
        }
    });
}

Example 29 with TransformNR

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

the class CartesianNamespacePidKinematics method setDesiredJointSpaceVector.

/**
 * This calculates the target pose
 * @param JointSpaceVector the target joint space vector
 * @param seconds the time for the transition to take from current position to target, unit seconds
 * @return The joint space vector is returned for target arrival referance
 * @throws Exception If there is a workspace error
 */
@Override
public double[] setDesiredJointSpaceVector(double[] jointSpaceVect, double seconds) throws Exception {
    if (jointSpaceVect.length != getNumberOfLinks()) {
        throw new IndexOutOfBoundsException("Vector must be " + getNumberOfLinks() + " links, actual number of links = " + jointSpaceVect.length);
    }
    factory.setCachedTargets(jointSpaceVect);
    currentJointSpaceTarget = jointSpaceVect;
    TransformNR fwd = connection.setDesiredJointSpaceVector(jointSpaceVect, seconds);
    fireTargetJointsUpdate(currentJointSpaceTarget, fwd);
    return jointSpaceVect;
}

Example 30 with TransformNR

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

the class CartesianNamespacePidKinematics method getCurrentTaskSpaceTransform.

/**
 * This takes a reading of the robots position and converts it to a joint space vector
 * This vector is converted to task space and returned
 * @return taskSpaceVector in mm,radians [x,y,z,rotx,rotY,rotZ]
 */
@Override
public TransformNR getCurrentTaskSpaceTransform() {
    // TransformNR fwd  = forwardKinematics(getCurrentJointSpaceVector());
    TransformNR fwd = connection.getCurrentTaskSpaceTransform();
    // update the joint space
    getCurrentJointSpaceVector();
    // Log.info("Getting robot task space "+fwd);
    TransformNR taskSpaceTransform = forwardOffset(fwd);
    // Log.info("Getting global task space "+taskSpaceTransform);
    return taskSpaceTransform;
}