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

Example 11 with TransformNR

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

the class AbstractKinematicsNR 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
 */
public synchronized 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);
    }
    String joints = "[";
    for (int i = 0; i < jointSpaceVect.length; i++) {
        joints += jointSpaceVect[i] + " ";
    }
    joints += "]";
    Log.info("Setting target joints: " + joints);
    int except = 0;
    Exception e = null;
    do {
        try {
            factory.setCachedTargets(jointSpaceVect);
            if (!isNoFlush()) {
                // 
                factory.flush(seconds);
            // 
            }
            except = 0;
            e = null;
        } catch (Exception ex) {
            except++;
            e = ex;
        }
    } while (except > 0 && except < getRetryNumberBeforeFail());
    if (e != null)
        throw e;
    // for(int i=0;i<getNumberOfLinks();i++){
    // setDesiredJointAxisValue(i, jointSpaceVect[i],  seconds);
    // }
    currentJointSpaceTarget = jointSpaceVect;
    TransformNR fwd = forwardKinematics(currentJointSpaceTarget);
    fireTargetJointsUpdate(currentJointSpaceTarget, fwd);
    return jointSpaceVect;
}

Example 12 with TransformNR

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

the class AbstractKinematicsNR method forwardOffset.

protected TransformNR forwardOffset(TransformNR t) {
    Matrix btt = getRobotToFiducialTransform().getMatrixTransform();
    Matrix ftb = getFiducialToGlobalTransform().getMatrixTransform();
    Matrix current = t.getMatrixTransform();
    Matrix mForward = ftb.times(btt).times(current);
    return new TransformNR(mForward);
}

Example 13 with TransformNR

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

the class AbstractKinematicsNR method setGlobalToFiducialTransform.

public void setGlobalToFiducialTransform(TransformNR frameToBase) {
    Log.info("Setting Global To Fiducial Transform " + frameToBase);
    this.fiducial2RAS = frameToBase;
    for (IRegistrationListenerNR r : regListeners) {
        r.onFiducialToGlobalUpdate(this, frameToBase);
    }
    Platform.runLater(new Runnable() {

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

Example 14 with TransformNR

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

the class AbstractKinematicsNR method loadConfig.

/**
 * Load XML configuration file,
 * then store in LinkConfiguration (ArrayList type)
 */
protected ArrayList<LinkConfiguration> loadConfig(Element doc) {
    ArrayList<LinkConfiguration> localConfigsFromXml = new ArrayList<LinkConfiguration>();
    NodeList nodListofLinks = doc.getChildNodes();
    setCadEngine(getGistCodes(doc, "cadEngine"));
    setDhEngine(getGistCodes(doc, "kinematics"));
    for (int i = 0; i < nodListofLinks.getLength(); i++) {
        Node linkNode = nodListofLinks.item(i);
        if (linkNode.getNodeType() == Node.ELEMENT_NODE && linkNode.getNodeName().contentEquals("link")) {
            localConfigsFromXml.add(new LinkConfiguration((Element) linkNode));
            NodeList dHParameters = linkNode.getChildNodes();
            for (int x = 0; x < dHParameters.getLength(); x++) {
                Node nNode = dHParameters.item(x);
                if (nNode.getNodeType() == Node.ELEMENT_NODE && nNode.getNodeName().contentEquals("DHParameters")) {
                    Element dhNode = (Element) nNode;
                    DHLink newLink = new DHLink(dhNode);
                    // 0->1
                    getDhParametersChain().addLink(newLink);
                    NodeList mobileBasesNodeList = dhNode.getChildNodes();
                    for (int j = 0; j < mobileBasesNodeList.getLength(); j++) {
                        Node mb = mobileBasesNodeList.item(j);
                        if (mb.getNodeType() == Node.ELEMENT_NODE && mb.getNodeName().contentEquals("mobilebase")) {
                            final MobileBase newMobileBase = new MobileBase((Element) mb);
                            mobileBases.add(newMobileBase);
                            newLink.setMobileBaseXml(newMobileBase);
                            newLink.addDhLinkPositionListener(new IDhLinkPositionListener() {

                                @Override
                                public void onLinkGlobalPositionChange(TransformNR newPose) {
                                    Log.debug("Motion in the D-H link has caused this mobile base to move");
                                    newMobileBase.setGlobalToFiducialTransform(newPose);
                                }
                            });
                        }
                    }
                }
            }
        } else if (linkNode.getNodeType() == Node.ELEMENT_NODE && linkNode.getNodeName().contentEquals("name")) {
            try {
                setScriptingName(XmlFactory.getTagValue("name", doc));
            } catch (Exception E) {
                E.printStackTrace();
            }
        } else if (linkNode.getNodeType() == Node.ELEMENT_NODE && linkNode.getNodeName().contentEquals("ZframeToRAS")) {
            Element eElement = (Element) linkNode;
            setZframeToGlobalTransform(new TransformNR(Double.parseDouble(XmlFactory.getTagValue("x", eElement)), Double.parseDouble(XmlFactory.getTagValue("y", eElement)), Double.parseDouble(XmlFactory.getTagValue("z", eElement)), new RotationNR(new double[] { Double.parseDouble(XmlFactory.getTagValue("rotw", eElement)), Double.parseDouble(XmlFactory.getTagValue("rotx", eElement)), Double.parseDouble(XmlFactory.getTagValue("roty", eElement)), Double.parseDouble(XmlFactory.getTagValue("rotz", eElement)) })));
        } else if (linkNode.getNodeType() == Node.ELEMENT_NODE && linkNode.getNodeName().contentEquals("baseToZframe")) {
            Element eElement = (Element) linkNode;
            setBaseToZframeTransform(new TransformNR(Double.parseDouble(XmlFactory.getTagValue("x", eElement)), Double.parseDouble(XmlFactory.getTagValue("y", eElement)), Double.parseDouble(XmlFactory.getTagValue("z", eElement)), new RotationNR(new double[] { Double.parseDouble(XmlFactory.getTagValue("rotw", eElement)), Double.parseDouble(XmlFactory.getTagValue("rotx", eElement)), Double.parseDouble(XmlFactory.getTagValue("roty", eElement)), Double.parseDouble(XmlFactory.getTagValue("rotz", eElement)) })));
        } else {
            // System.err.println(linkNode.getNodeName());
            Log.error("Node not known: " + linkNode.getNodeName());
        }
    }
    return localConfigsFromXml;
}

Example 15 with TransformNR

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

the class CartesianNamespacePidKinematics method setDesiredJointAxisValue.

/**
 * Sets an individual target joint position
 * @param axis the joint index to set
 * @param value the value to set it to
 * @param seconds the time for the transition to take from current position to target, unit seconds
 * @throws Exception If there is a workspace error
 */
@Override
public void setDesiredJointAxisValue(int axis, double value, double seconds) throws Exception {
    LinkConfiguration c = getLinkConfiguration(axis);
    Log.info("Setting single target joint in mm/deg, axis=" + axis + " value=" + value);
    currentJointSpaceTarget[axis] = value;
    try {
        getFactory().getLink(c).setTargetEngineeringUnits(value);
    } catch (Exception ex) {
        throw new Exception("Joint hit software bound, index " + axis + " attempted: " + value + " boundes: U=" + c.getUpperLimit() + ", L=" + c.getLowerLimit());
    }
    if (!isNoFlush()) {
        int except = 0;
        Exception e = null;
        do {
            try {
                getFactory().getLink(c).flush(seconds);
                except = 0;
                e = null;
            } catch (Exception ex) {
                except++;
                e = ex;
            }
        } while (except > 0 && except < getRetryNumberBeforeFail());
        if (e != null)
            throw e;
    }
    TransformNR fwd = connection.getCurrentTaskSpaceTransform();
    fireTargetJointsUpdate(currentJointSpaceTarget, fwd);
    return;
}