DHParameterKinematics.java

Go to the documentation of this file.

package com.neuronrobotics.sdk.addons.kinematics;
 
import java.io.File;
import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.InputStream;
import java.util.ArrayList;
 
import org.w3c.dom.Element;
 
import Jama.Matrix;
import javafx.scene.transform.Affine;
 
import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR;
import com.neuronrobotics.sdk.addons.kinematics.xml.XmlFactory;
import com.neuronrobotics.sdk.common.BowlerAbstractDevice;
import com.neuronrobotics.sdk.common.IDeviceConnectionEventListener;
 
// TODO: Auto-generated Javadoc
public class DHParameterKinematics extends AbstractKinematicsNR
        implements ITaskSpaceUpdateListenerNR, IJointSpaceUpdateListenerNR {
 
    private DHChain chain = null;
 
    private ArrayList<Object> linksListeners = new ArrayList<Object>();
 
    private Object currentTarget = new Object();
 
    boolean disconnecting = false;
 
    IDeviceConnectionEventListener l = new IDeviceConnectionEventListener() {
        @Override
        public void onDisconnect(BowlerAbstractDevice source) {
            if (!disconnecting) {
                disconnecting = true;
                //disconnect();
            }
 
        }
 
        @Override
        public void onConnect(BowlerAbstractDevice source) {
        }
    };
 
    private ArrayList<LinkConfiguration> configs;
 
    public DHParameterKinematics(BowlerAbstractDevice bad, Element linkStream) {
        super(linkStream, new LinkFactory(bad));
        setChain(getDhParametersChain());
        for (LinkConfiguration lf : getFactory().getLinkConfigurations())
            if (getFactory().getDyio(lf) != null) {
                getFactory().getDyio(lf).addConnectionEventListener(l);
                return;
            }
        addConnectionEventListener(new IDeviceConnectionEventListener() {
            
            @Override
            public void onDisconnect(BowlerAbstractDevice source) {
                for(int i=0;i<getNumberOfLinks();i++) {
                    MobileBase m = getSlaveMobileBase(i);
                    if(m!=null)
                        m.disconnect();
                }
            }
            
            @Override
            public void onConnect(BowlerAbstractDevice source) {
                // TODO Auto-generated method stub
                
            }
        });
    }
 
    public DHParameterKinematics(BowlerAbstractDevice bad, InputStream linkStream) {
        super(linkStream, new LinkFactory(bad));
        setChain(getDhParametersChain());
        for (LinkConfiguration lf : getFactory().getLinkConfigurations())
            if (getFactory().getDyio(lf) != null) {
                getFactory().getDyio(lf).addConnectionEventListener(l);
                return;
            }
    }
 
    @Deprecated
    public DHParameterKinematics(BowlerAbstractDevice bad, InputStream linkStream, InputStream depricated) {
        this(bad, linkStream);
    }
 
    public DHParameterKinematics(BowlerAbstractDevice bad) {
        this(bad, XmlFactory.getDefaultConfigurationStream("TrobotLinks.xml"));
    }
 
    public DHParameterKinematics(BowlerAbstractDevice bad, String file) {
        this(bad, XmlFactory.getDefaultConfigurationStream(file));
    }
 
    public DHParameterKinematics(BowlerAbstractDevice bad, File configFile) throws FileNotFoundException {
        this(bad, new FileInputStream(configFile));
    }
 
    public DHParameterKinematics() {
        this(null,(InputStream)null);
    }
 
    public DHParameterKinematics(String file) {
        this(null, XmlFactory.getDefaultConfigurationStream(file));
    }
 
    public DHParameterKinematics(Element linkStream) {
        this(null, linkStream);
    }
 
    public DHParameterKinematics(File configFile) throws FileNotFoundException {
        this(null, new FileInputStream(configFile));
    }
 
    /*
     * (non-Javadoc)
     * 
     * @see com.neuronrobotics.sdk.addons.kinematics.AbstractKinematicsNR#
     * inverseKinematics(com.neuronrobotics.sdk.addons.kinematics.math.TransformNR)
     */
    @Override
    public double[] inverseKinematics(TransformNR taskSpaceTransform) throws Exception {
        return getDhChain().inverseKinematics(taskSpaceTransform, getCurrentJointSpaceVector());
    }
 
    /*
     * (non-Javadoc)
     * 
     * @see com.neuronrobotics.sdk.addons.kinematics.AbstractKinematicsNR#
     * forwardKinematics(double[])
     */
    @Override
    public TransformNR forwardKinematics(double[] jointSpaceVector) {
        if (jointSpaceVector == null || getDhChain() == null)
            return new TransformNR();
        TransformNR rt = getDhChain().forwardKinematics(jointSpaceVector);
        return rt;
    }
    private double [] crossProduct(double[] a, double[] b){
        double [] xProd = new double [3];
        
        xProd[0]=a[1]*b[2]-a[2]*b[1];
        xProd[1]=a[2]*b[0]-a[0]*b[2];
        xProd[2]=a[0]*b[1]-a[1]*b[0];
        
        return xProd;
    }
    public Matrix getJacobian(DHChain chain, double[] jointSpaceVector, int index){
        int size = chain.getLinks().size();
        double [][] data = new double[6][size]; 
        chain.getChain(jointSpaceVector);
        for(int i=0;i<size;i++){
            if(i>index) continue;
            Matrix rotationComponent = forwardOffset(new TransformNR()).getMatrixTransform();
            for(int j=i;j<size && j<=index;j++) {
                double value=0;
                if(chain.getLinks().get(j).getLinkType()==DhLinkType.ROTORY)
                    value=Math.toRadians(jointSpaceVector[j]);
                else
                    value=jointSpaceVector[j];
                Matrix step = chain.getLinks().get(j).DhStep(value);
                //Log.info( "Current:\n"+current+"Step:\n"+step);
                //println i+" Link "+j+" index "+index+" step "+TransformNR.getMatrixString(step)
                rotationComponent = rotationComponent.times(step);
            }
            double [] zVect = new double [3];
            double [] zVectEnd = new double [3];
            double [][] rotation=new TransformNR(rotationComponent).getRotationMatrix().getRotationMatrix();
            zVectEnd[0]=rotation[2][2];
            zVectEnd[1]=rotation[2][1];
            zVectEnd[2]=rotation[2][0];
            if(i==0 && index ==0 ){
                zVect[0]=0;
                zVect[1]=0;
                zVect[2]=1;
            }else if(i<=index){
                //println "Link "+index+" "+TransformNR.getMatrixString(new Matrix(rotation))
                //Get the rz vector from matrix
                zVect[0]=zVectEnd[0];
                zVect[1]=zVectEnd[1];
                zVect[2]=zVectEnd[2];
            }else{
                zVect[0]=0;
                zVect[1]=0;
                zVect[2]=0;
            }
            //Assume all rotational joints
            //Set to zero if prismatic
            if(chain.getLinks().get(i).getLinkType()==DhLinkType.ROTORY){
                data[3][i]=zVect[0];
                data[4][i]=zVect[1];
                data[5][i]=zVect[2];
            }else{
                data[3][i]=0;
                data[4][i]=0;
                data[5][i]=0;
            }
            double []rVect = new double [3];            
            Matrix rComponentmx = forwardOffset(new TransformNR()).getMatrixTransform();
            for(int j=0;j<i ;j++) {
                double value=0;
                if(chain.getLinks().get(j).getLinkType()==DhLinkType.ROTORY)
                    value=Math.toRadians(jointSpaceVector[j]);
                else
                    value=jointSpaceVector[j];
                Matrix step = chain.getLinks().get(j).DhStep(value);
                //Log.info( "Current:\n"+current+"Step:\n"+step);
                //println i+" Link "+j+" index "+index+" step "+TransformNR.getMatrixString(step)
                rComponentmx = rComponentmx.times(step);
            }
            //Figure out the current 
            Matrix tipOffsetmx =forwardOffset( new TransformNR()).getMatrixTransform();
            for(int j=0;j<size && j<=index;j++) {
                double value=0;
                if(chain.getLinks().get(j).getLinkType()==DhLinkType.ROTORY)
                    value=Math.toRadians(jointSpaceVector[j]);
                else
                    value=jointSpaceVector[j];
                Matrix step = chain.getLinks().get(j).DhStep(value);
                //Log.info( "Current:\n"+current+"Step:\n"+step);
                //println i+" Link "+j+" index "+index+" step "+TransformNR.getMatrixString(step)
                tipOffsetmx = tipOffsetmx.times(step);
            }
            double []tipOffset = new double [3];
            double []rComponent = new double [3];
            TransformNR tipOffsetnr = new TransformNR(tipOffsetmx);//.times(myInvertedStarting);
            tipOffset[0]=tipOffsetnr.getX();
            tipOffset[1]=tipOffsetnr.getY();
            tipOffset[2]=tipOffsetnr.getZ();
            TransformNR rComponentnr = new TransformNR(rComponentmx);//.times(myInvertedStarting);
            rComponent[0]=rComponentnr.getX();
            rComponent[1]=rComponentnr.getY();
            rComponent[2]=rComponentnr.getZ();
            for(int x=0;x<3;x++)
                rVect[x]=(tipOffset[x]-rComponent[x]);
            //Cross product of rVect and Z vect
            double []xProd = crossProduct( zVect,rVect);
            data[0][i]=xProd[0];
            data[1][i]=xProd[1];
            data[2][i]=xProd[2];
        }
        //println "\n\n"
        return new Matrix(data);
    }
 
    public Matrix getJacobian() {
        return getJacobian(getDhChain().getLinks().size()-1);
    }
    public Matrix getJacobian(int index) {
        return getJacobian(getCurrentJointSpaceVector() , index) ;
    }
   public Matrix getJacobian(double[] jointSpaceVector,int index) {
 
       return  getJacobian(getDhChain() ,  jointSpaceVector,  index);
   }
    public ArrayList<TransformNR> getChainTransformations() {
        return getChain().getChain(getCurrentJointSpaceVector());
    }
 
    public void setDhChain(DHChain chain) {
        this.setChain(chain);
    }
 
    public DHChain getDhChain() {
        return getChain();
    }
 
    public DHChain getChain() {
        return chain;
    }
 
    public void setChain(DHChain chain) {
        this.chain = chain;
        ArrayList<DHLink> dhLinks = chain.getLinks();
//      for (int i = linksListeners.size(); i < dhLinks.size(); i++) {
//          linksListeners.add(new Object());
//      }
        LinkFactory lf = getFactory();
        configs = lf.getLinkConfigurations();
        for (int i = 0; i < dhLinks.size(); i++) {
            //dhLinks.get(i).setListener(linksListeners.get(i));
            dhLinks.get(i).setRootListener(getRootListener());
            // This mapps together the position of the links in the kinematics and the link
            // actions themselves (used for cameras and tools)
            //lf.getLink(configs.get(i)).setGlobalPositionListener(linksListeners.get(i));
            if (getLinkConfiguration(i).isTool()) {
                dhLinks.get(i).setLinkType(DhLinkType.TOOL);
            } else if (getLinkConfiguration(i).isPrismatic())
                dhLinks.get(i).setLinkType(DhLinkType.PRISMATIC);
            else
                dhLinks.get(i).setLinkType(DhLinkType.ROTORY);
        }
        addPoseUpdateListener(this);
        addJointSpaceListener(this);
    }
 
    /*
     * (non-Javadoc)
     * 
     * @see com.neuronrobotics.sdk.addons.kinematics.AbstractKinematicsNR#getXml()
     */
    /*
     * 
     * Generate the xml configuration to generate an XML of this robot.
     */
    public String getXml() {
        String xml = "<root>\n";
        xml += getEmbedableXml();
        xml += "\n</root>";
        return xml;
    }
 
    /*
     * 
     * Generate the xml configuration to generate an XML of this robot.
     */
    public String getEmbedableXml() {
 
        String xml = "";
 
        xml += "\t<cadEngine>\n";
        xml += "\t\t<git>" + getGitCadEngine()[0] + "</git>\n";
        xml += "\t\t<file>" + getGitCadEngine()[1] + "</file>\n";
        xml += "\t</cadEngine>\n";
 
        xml += "\t<kinematics>\n";
        xml += "\t\t<git>" + getGitDhEngine()[0] + "</git>\n";
        xml += "\t\t<file>" + getGitDhEngine()[1] + "</file>\n";
        xml += "\t</kinematics>\n";
 
        ArrayList<DHLink> dhLinks = chain.getLinks();
        for (int i = 0; i < dhLinks.size(); i++) {
            xml += "<link>\n";
            xml += getLinkConfiguration(i).getXml();
            xml += dhLinks.get(i).getXml();
            xml += "\n</link>\n";
        }
        xml += "\n<ZframeToRAS\n>";
        xml += getFiducialToGlobalTransform().getXml();
        xml += "\n</ZframeToRAS>\n";
 
        xml += "\n<baseToZframe>\n";
        xml += getRobotToFiducialTransform().getXml();
        xml += "\n</baseToZframe>\n";
        return xml;
    }
 
    /*
     * (non-Javadoc)
     * 
     * @see com.neuronrobotics.sdk.addons.kinematics.AbstractKinematicsNR#
     * disconnectDevice()
     */
    @Override
    public void disconnectDevice() {
        // TODO Auto-generated method stub
        removePoseUpdateListener(this);
        removeJointSpaceUpdateListener(this);
    }
 
    /*
     * (non-Javadoc)
     * 
     * @see
     * com.neuronrobotics.sdk.addons.kinematics.AbstractKinematicsNR#connectDevice()
     */
    @Override
    public boolean connectDevice() {
        // TODO Auto-generated method stub
        return true;
    }
 
    /*
     * (non-Javadoc)
     * 
     * @see com.neuronrobotics.sdk.addons.kinematics.ITaskSpaceUpdateListenerNR#
     * onTaskSpaceUpdate(com.neuronrobotics.sdk.addons.kinematics.
     * AbstractKinematicsNR,
     * com.neuronrobotics.sdk.addons.kinematics.math.TransformNR)
     */
    @Override
    public void onTaskSpaceUpdate(AbstractKinematicsNR source, TransformNR pose) {
 
    }
 
    /*
     * (non-Javadoc)
     * 
     * @see com.neuronrobotics.sdk.addons.kinematics.ITaskSpaceUpdateListenerNR#
     * onTargetTaskSpaceUpdate(com.neuronrobotics.sdk.addons.kinematics.
     * AbstractKinematicsNR,
     * com.neuronrobotics.sdk.addons.kinematics.math.TransformNR)
     */
    @Override
    public void onTargetTaskSpaceUpdate(AbstractKinematicsNR source, TransformNR pose) {
        // TODO Auto-generated method stub
        // TransformFactory.getTransform(pose, getCurrentTargetObject());
    }
 
    public DhInverseSolver getInverseSolver() {
        return chain.getInverseSolver();
    }
 
    public void setInverseSolver(DhInverseSolver inverseSolver) {
        chain.setInverseSolver(inverseSolver);
    }
 
    public Object getCurrentTargetObject() {
        return currentTarget;
    }
 
    public void addNewLink(LinkConfiguration newLink, DHLink dhLink) {
        LinkFactory factory = getFactory();
        // remove the link listener while the number of links could chnage
        factory.removeLinkListener(this);
        AbstractLink link = factory.getLink(newLink);// adds new link internally
        if(dhLink.getListener()==null)
            throw new RuntimeException("FAIL the link listner must be set to        dhLink.setListener(new Affine());");
        link.setGlobalPositionListener(dhLink.getListener());
        DHChain chain = getDhChain();
        chain.addLink(dhLink);
        // set the modified kinematics chain
        setChain(chain);
        if (newLink.isTool()) {
            dhLink.setLinkType(DhLinkType.TOOL);
        } else if (newLink.isPrismatic())
            dhLink.setLinkType(DhLinkType.PRISMATIC);
        else
            dhLink.setLinkType(DhLinkType.ROTORY);
        // once the new link configuration is set up, re add the listener
        factory.addLinkListener(this);
        chain.setFactory(factory);
 
        link.flush(0);
    }
 
    public void removeLink(int index) {
        LinkFactory factory = getFactory();
        // remove the link listener while the number of links could chnage
        factory.removeLinkListener(this);
        DHChain chain = getDhChain();
        chain.getLinks().remove(index);
        factory.deleteLink(index);
        // set the modified kinematics chain
        setChain(chain);
        // once the new link configuration is set up, re add the listener
        factory.addLinkListener(this);
    }
 
    public ArrayList<TransformNR>  updateCadLocations() {
            try {
                ArrayList<TransformNR> ll = getChain().getChain(getCurrentJointSpaceVector());
                return ll;
            } catch (Exception ex) {
                // ex.printStackTrace();
            }
 
            return null;
    }
 
    /*
     * (non-Javadoc)
     * 
     * @see com.neuronrobotics.sdk.addons.kinematics.IJointSpaceUpdateListenerNR#
     * onJointSpaceUpdate(com.neuronrobotics.sdk.addons.kinematics.
     * AbstractKinematicsNR, double[])
     */
    @Override
    public void onJointSpaceUpdate(final AbstractKinematicsNR source, final double[] joints) {
        ArrayList<TransformNR> cached=updateCadLocations();
        if(cached!=null)
            for(int i=0;i<joints.length;i++) {
                DHLink dhLink = getChain().getLinks().get(i);
                TransformNR newPose = cached.get(i);
                dhLink.fireOnLinkGlobalPositionChange(newPose);
            }   
    }
 
    @Override
    public void setGlobalToFiducialTransform(TransformNR frameToBase) {
        super.setGlobalToFiducialTransform(frameToBase);
        if(getChain()!=null) {
            getChain().setChain(null);// force an update of teh cached locations because base changed
            try {
                getChain().getChain(getCurrentJointSpaceVector());//calculate new locations
            }catch(Exception e) {
                throw new RuntimeException("Limb "+getScriptingName()+", "+e.getMessage());
            }
        }
        runRenderWrangler();
    }
 
    /*
     * (non-Javadoc)
     * 
     * @see com.neuronrobotics.sdk.addons.kinematics.IJointSpaceUpdateListenerNR#
     * onJointSpaceTargetUpdate(com.neuronrobotics.sdk.addons.kinematics.
     * AbstractKinematicsNR, double[])
     */
    @Override
    public void onJointSpaceTargetUpdate(AbstractKinematicsNR source, double[] joints) {
        // TODO Auto-generated method stub
 
    }
 
    /*
     * (non-Javadoc)
     * 
     * @see com.neuronrobotics.sdk.addons.kinematics.IJointSpaceUpdateListenerNR#
     * onJointSpaceLimit(com.neuronrobotics.sdk.addons.kinematics.
     * AbstractKinematicsNR, int,
     * com.neuronrobotics.sdk.addons.kinematics.JointLimit)
     */
    @Override
    public void onJointSpaceLimit(AbstractKinematicsNR source, int axis, JointLimit event) {
        // TODO Auto-generated method stub
 
    }
 
    // New helper functions
    
    public TransformNR linkCoM(double linkAngleToClaculate, int linkIndex) {
        double[] vectortail = getCurrentJointSpaceVector();
        vectortail[linkIndex] = linkAngleToClaculate;
        return getChain().getChain(vectortail).get(linkIndex)
                .times(getLinkConfiguration(linkIndex).getCenterOfMassFromCentroid());
 
    }
    
    public TransformNR linkCoM(int linkIndex) {
        return linkCoM(getCurrentJointSpaceVector()[linkIndex],linkIndex);
    }
    public Object getLinkObjectManipulator(int index) {
         return getChain().getLinks().get(index).getListener();
    }
    public double getDH_Theta(int index) {
        return getChain().getLinks().get(index).getTheta();
    }
    public double getDH_D(int index) {
        return getChain().getLinks().get(index).getDelta();
    }
 
 
 
    public double getDH_R(int index) {
        return getChain().getLinks().get(index).getRadius();
    }
 
    public double getDH_Alpha(int index) {
        return getChain().getLinks().get(index).getAlpha();
    }
    
    public void setDH_Theta(int index, double value) {
         getChain().getLinks().get(index).setTheta(value);
    }
    public void setDH_D(int index, double value) {
         getChain().getLinks().get(index).setDelta(value);
    }
 
 
 
    public void setDH_R(int index, double value) {
         getChain().getLinks().get(index).setRadius(value);
    }
 
    public void setDH_Alpha(int index, double value) {
         getChain().getLinks().get(index).setAlpha(value);
    }
 
    public DHLink getDhLink(int i) {
        return getDhChain().getLinks().get(i);
    }
    public Object getListener(int i) {
        return getDhChain().getLinks().get(i).getListener();
    }
    @Override
    public void setRobotToFiducialTransform(TransformNR newTrans) {
        super.setRobotToFiducialTransform(newTrans);
    }
    
    public void refreshPose() {
        runRenderWrangler();
    }
    public MobileBase getSlaveMobileBase(int index) {
        return getDhLink(index).getSlaveMobileBase();
    }
    public void throwExceptionOnJointLimit(boolean b) {
        for(int i=0;i<getNumberOfLinks();i++) {
            getAbstractLink(i).setUseLimits(b);
        }
    }
 
    public TransformNR getLinkTip(int linkIndex) {
        return getChain().getCachedChain().get(linkIndex);
    }
}