DHChain.java

Go to the documentation of this file.

package com.neuronrobotics.sdk.addons.kinematics;
 
import java.io.InputStream;
import java.util.ArrayList;
 
 
 
import org.w3c.dom.Element;
import org.w3c.dom.Node;
import org.w3c.dom.NodeList;
 
import Jama.Matrix;
 
import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR;
import com.neuronrobotics.sdk.addons.kinematics.xml.XmlFactory;
import com.neuronrobotics.sdk.common.Log;
// TODO: Auto-generated Javadoc
 
public  class DHChain {
    
    private ArrayList<DHLink> links = new ArrayList<DHLink>();
    
    private ArrayList<TransformNR> chain = new ArrayList<TransformNR>();
    
    private ArrayList<TransformNR> intChain = new ArrayList<TransformNR>();
    
    private double[] upperLimits;
    
    private double[] lowerLimits;
    
    private boolean debug=false;
    
    private DhInverseSolver is;
    
    public AbstractKinematicsNR kin;
    
    private LinkFactory factory;
 
 
    public DHChain( AbstractKinematicsNR kin){
        this.kin = kin;
 
    }
    
    public void addLink(DHLink link){
        if(!getLinks().contains(link)){
            getLinks().add(link);
        }
    }
    
    public void removeLink(DHLink link){
        if(getLinks().contains(link)){
            getLinks().remove(link);
        }
    }
    
    
//  public DHChain(double [] upperLimits,double [] lowerLimits, boolean debugViewer ) {
//      this(upperLimits, lowerLimits);
//
//  }
//  
//  public DHChain(double [] upperLimits,double [] lowerLimits ) {
//      
//      this.upperLimits = upperLimits;
//      this.lowerLimits = lowerLimits;
//      getLinks().add(new DHLink(  13,     Math.toRadians(180),    32,     Math.toRadians(-90)));//0->1
//      getLinks().add(new DHLink(  25,     Math.toRadians(-90),    93,     Math.toRadians(180)));//1->2
//      getLinks().add(new DHLink(  11,     Math.toRadians(90),     24,     Math.toRadians(90)));//2->3 
//      getLinks().add(new DHLink(  128,    Math.toRadians(-90),        0,      Math.toRadians(90)));//3->4
//      
//      getLinks().add(new DHLink(  0,      Math.toRadians(0),          0,      Math.toRadians(-90)));//4->5
//      getLinks().add(new DHLink(  25,     Math.toRadians(90),         0,      Math.toRadians(0)));//5->tool
//      
//      forwardKinematics(new  double [] {0,0,0,0,0,0});
//  }
 
public double[] inverseKinematics(TransformNR target,double[] jointSpaceVector )throws Exception {
        
        if(getLinks() == null)
            return null;
        long start = System.currentTimeMillis();
        
        
        //is = new GradiantDecent(this,debug);
        //is = new SearchTreeSolver(this,debug);
        if(getInverseSolver() == null)
            setInverseSolver(new ComputedGeometricModel(this,debug));
        
        double [] inv = getInverseSolver().inverseKinematics(target, jointSpaceVector,this);    
        if(debug){
            //getViewer().updatePoseDisplay(getChain(jointSpaceVector));
        }
        
        //Log.info( "Inverse Kinematics took "+(System.currentTimeMillis()-start)+"ms");
        return inv;
    }
 
    public TransformNR forwardKinematics(double[] jointSpaceVector) {
        return forwardKinematics(jointSpaceVector, true);
    }
    
    public TransformNR forwardKinematics(double[] jointSpaceVector, boolean store) {
        return new TransformNR(forwardKinematicsMatrix(jointSpaceVector, store) );
    }
    
 
    
    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 forwardKinematicsMatrix(double[] jointSpaceVector, boolean store) {
        return forwardKinematicsMatrix(jointSpaceVector,store?getCachedChain():null);
    }
    public Matrix forwardKinematicsMatrix(double[] jointSpaceVector,    ArrayList<TransformNR>  chainToLoad) {
        if(getLinks() == null)
            return new TransformNR().getMatrixTransform();
        if (jointSpaceVector.length!=getLinks().size())
            throw new IndexOutOfBoundsException("DH links do not match defined links. expected "+getLinks().size()+" got "+jointSpaceVector.length);
        Matrix current = new TransformNR().getMatrixTransform();
        
        for(int i=0;i<getLinks().size();i++) {
            LinkFactory factory2 = getFactory();
            ArrayList<LinkConfiguration> linkConfigurations = factory2.getLinkConfigurations();
            LinkConfiguration conf= linkConfigurations.get(i);
            Matrix step;
            if(conf.isPrismatic())
                step= getLinks().get(i).DhStep(jointSpaceVector[i]);
            else
                step= getLinks().get(i).DhStep(Math.toRadians(jointSpaceVector[i]));
            //Log.info( "Current:\n"+current+"Step:\n"+step);
            current = current.times(step);
            try {
                final TransformNR pose =forwardOffset(new TransformNR(current));
                
                
    
                if(chainToLoad!=null){
                    if(intChain.size()<=i)
                        intChain.add(new TransformNR(step));
                    else{
                        intChain.set(i, new TransformNR(step));
                    }
                    
                    if(chainToLoad.size()<=i)
                        chainToLoad.add(pose);
                    else{
                        chainToLoad.set(i, pose);
                    }
                }
            }catch(java.lang.RuntimeException ex) {
                String pose="[\n";
                for(int x=0;x<jointSpaceVector.length;x++) {
                    pose=pose+" "+jointSpaceVector[x]+" , \n";
                }
                pose+="]";
                throw new RuntimeException(ex.getMessage()+" for pose "+pose);
            }
        }
        //Log.info( "Final:\n"+current);
        return current;
    }
    
    private TransformNR forwardOffset(TransformNR transformNR) {
        return kin.forwardOffset(transformNR);
    }
 
    public void setChain(ArrayList<TransformNR> chain) {
        if(chain!=null)
            this.chain = chain;
        getCachedChain().clear();
        //else
        //  new RuntimeException().printStackTrace();
    }
 
    public ArrayList<TransformNR> getChain(double[] jointSpaceVector) {
        ArrayList<TransformNR> chainToLoad = new ArrayList<TransformNR>();
        forwardKinematicsMatrix(jointSpaceVector,chainToLoad);
        return chainToLoad;
    }
    
    public ArrayList<TransformNR> getCachedChain() {
        if(chain==null)
            chain=new ArrayList<TransformNR>();
        return chain;
    }
    
    public double[] getUpperLimits() {
        // TODO Auto-generated method stub
        return upperLimits;
    }
 
    public double[] getlowerLimits() {
        // TODO Auto-generated method stub
        return lowerLimits;
    }
 
    public void setLinks(ArrayList<DHLink> links) {
        this.links = links;
    }
 
    public ArrayList<DHLink> getLinks() {
        return links;
    }
    
    /* (non-Javadoc)
     * @see java.lang.Object#toString()
     */
    public String toString(){
        String s="";
        for(DHLink l:getLinks()){
            s+=l.toString()+"\n";
        }
        return s;
                
    }
 
    public DhInverseSolver getInverseSolver() {
        if(is==null){
            is=new DhInverseSolver() {
                
                @Override
                public double[] inverseKinematics(TransformNR target,
                        double[] jointSpaceVector,DHChain chain ) {
                    ArrayList<DHLink> links = chain.getLinks();
                    // THis is the jacobian for the given configuration
                    //Matrix jacobian =  chain.getJacobian(jointSpaceVector);
                    Matrix taskSpacMatrix = target.getMatrixTransform();
                    
                    int linkNum = jointSpaceVector.length;
                    double [] inv = new double[linkNum];
                    // this is an ad-hock kinematic model for d-h parameters and only works for specific configurations
                    
                    double d = links.get(1).getD()- links.get(2).getD();
                    double r = links.get(0).getR();
 
                    double lengthXYPlaneVect = Math.sqrt(Math.pow(target.getX(),2)+Math.pow(target.getY(),2));
                    double angleXYPlaneVect = Math.asin(target.getY()/lengthXYPlaneVect);
                    
                    double angleRectangleAdjustedXY =Math.asin(d/lengthXYPlaneVect);
                    
                    double lengthRectangleAdjustedXY = lengthXYPlaneVect* Math.cos(angleRectangleAdjustedXY)-r;
                    
                    
                    double orentation = angleXYPlaneVect-angleRectangleAdjustedXY;
                    if(Math.abs(Math.toDegrees(orentation))<0.01){
                        orentation=0;
                    }
                    double ySet = lengthRectangleAdjustedXY*Math.sin(orentation);
                    double xSet = lengthRectangleAdjustedXY*Math.cos(orentation);
                
                    
                    double zSet = target.getZ() - links.get(0).getD();
                    if(links.size()>4){
                        zSet+=links.get(4).getD();
                    }
                    // Actual target for anylitical solution is above the target minus the z offset
                    TransformNR overGripper = new TransformNR(
                            xSet,
                            ySet,
                            zSet,
                            target.getRotation());
 
 
                    double l1 = links.get(1).getR();// First link length
                    double l2 = links.get(2).getR();
 
                    double vect = Math.sqrt(xSet*xSet+ySet*ySet+zSet*zSet);
                    /*
                    println ( "TO: "+target);
                    println ( "Trangular TO: "+overGripper);
                    println ( "lengthXYPlaneVect: "+lengthXYPlaneVect);
                    println( "angleXYPlaneVect: "+Math.toDegrees(angleXYPlaneVect));
                    println( "angleRectangleAdjustedXY: "+Math.toDegrees(angleRectangleAdjustedXY));
                    println( "lengthRectangleAdjustedXY: "+lengthRectangleAdjustedXY);
                    println( "r: "+r);
                    println( "d: "+d);
                    
                    println( "x Correction: "+xSet);
                    println( "y Correction: "+ySet);
                    
                    println( "Orentation: "+Math.toDegrees(orentation));
                    println( "z: "+zSet);
                */
                    
 
                    if (vect > l1+l2 ||  vect<0 ||lengthRectangleAdjustedXY<0 ) {
                        throw new RuntimeException("Hypotenus too long: "+vect+" longer then "+l1+l2);
                    }
                    //from https://www.mathsisfun.com/algebra/trig-solving-sss-triangles.html
                    double a=l2;
                    double b=l1;
                    double c=vect;
                    double A =Math.acos((Math.pow(b,2)+ Math.pow(c,2) - Math.pow(a,2)) / (2.0*b*c));
                    double B =Math.acos((Math.pow(c,2)+ Math.pow(a,2) - Math.pow(b,2)) / (2.0*a*c));
                    double C =Math.PI-A-B;//Rule of triangles
                    double elevation = Math.asin(zSet/vect);
 
            /*
                    println( "vect: "+vect);
                    println( "A: "+Math.toDegrees(A));
                    println( "elevation: "+Math.toDegrees(elevation));
                    println( "l1 from x/y plane: "+Math.toDegrees(A+elevation));
                    println( "l2 from l1: "+Math.toDegrees(C));
                    */
                    inv[0] = Math.toDegrees(orentation);
                    inv[1] = -Math.toDegrees((A+elevation+links.get(1).getTheta()));
                    if((int)links.get(1).getAlpha() ==180){
                        inv[2] = (Math.toDegrees(C))-180-//interior angle of the triangle, map to external angle
                                Math.toDegrees(links.get(2).getTheta());// offset for kinematics
                    }
                    if((int)links.get(1).getAlpha() ==0){
                        inv[2] = -(Math.toDegrees(C))+Math.toDegrees(links.get(2).getTheta());// offset for kinematics
                    }
                    if(links.size()>3)
                        inv[3] =(inv[1] -inv[2]);// keep it parallell
                        // We know the wrist twist will always be 0 for this model
                    if(links.size()>4)
                        inv[4] = inv[0];//keep the tool orentation paralell from the base
                    
                    for(int i=0;i<inv.length;i++){
                        if(Math.abs(inv[i]) < 0.01){
                            inv[i]=0;
                        }
//                      println( "Link#"+i+" is set to "+inv[i]);
                    }
                    int i=3;
                    if(links.size()>3)
                        i=5;
                    //copy over remaining links so they do not move
                    for(;i<inv.length && i<jointSpaceVector.length ;i++){
                        inv[i]=jointSpaceVector[i];
                    }
                    return inv;
                }
            };
 
        }
        return is;
    }
 
    public void setInverseSolver(DhInverseSolver is) {
        this.is = is;
    }
 
    public LinkFactory getFactory() {
        return factory;
    }
 
    public void setFactory(LinkFactory factory) {
        upperLimits = factory.getUpperLimits();
        lowerLimits = factory.getLowerLimits();
        this.factory = factory;
    }
 
}