DHLink.java

Go to the documentation of this file.

package com.neuronrobotics.sdk.addons.kinematics;
 
import java.util.ArrayList;
 
 
import org.w3c.dom.Element;
 
import Jama.Matrix;
 
import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR;
import com.neuronrobotics.sdk.addons.kinematics.xml.XmlFactory;
 
// TODO: Auto-generated Javadoc
public class DHLink {
    
    private double d;
    
    private double theta;
    
    private double radius;
    
    private double alpha;
    
    private Matrix transX;
    
    private Matrix rotX;
    
    private Matrix transZ;
    
    private Matrix rotZ;
    
    private Matrix transX_J;
    
    private Matrix rotX_J;
    
    private Matrix transZ_J;
    
    private Matrix rotZ_J;
    
    private Object listener=null;
    
    private Object root=null;
    
    private DhLinkType type = DhLinkType.ROTORY;
    
    private ArrayList<IDhLinkPositionListener> dhlisteners = new ArrayList<IDhLinkPositionListener>();
    
    private MobileBase slaveMobileBase=null;
 
    private LinkConfiguration newLinkConf;
    
    
    public DHLink(double d, double theta,double r, double alpha) {
        this.setDelta(d);
        this.setTheta(theta);
        this.setRadius(r);
        this.setAlpha(alpha);
        
    }
 
    public DHLink(Element nNode, LinkConfiguration newLinkConf) {
        this.newLinkConf = newLinkConf;
        setDelta(XmlFactory.getTagValueDouble("Delta", nNode));
        setTheta(Math.toRadians(XmlFactory.getTagValueDouble("Theta", nNode)));
        setRadius(XmlFactory.getTagValueDouble("Radius", nNode));
        setAlpha(Math.toRadians(XmlFactory.getTagValueDouble("Alpha", nNode)));
        
    }
    
    public DHLink(DHLink dhl) {
        setDelta(dhl.getDelta());
        setTheta(dhl.getTheta());
        setRadius(dhl.getRadius());
        setAlpha(dhl.getAlpha());
    }
 
    public void fireOnLinkGlobalPositionChange(TransformNR newPose){
        if(slaveMobileBase!=null)
            slaveMobileBase.setGlobalToFiducialTransform(newPose);
        for(IDhLinkPositionListener l:dhlisteners){
            l.onLinkGlobalPositionChange(newPose);
        }
    }
    
    public void addDhLinkPositionListener(IDhLinkPositionListener l){
        if(!dhlisteners.contains(l))
            dhlisteners.add(l);
    }
    
    public void removeDhLinkPositionListener(IDhLinkPositionListener l){
        if(dhlisteners.contains(l))
            dhlisteners.remove(l);
    }
    
    /*
     * 
     * Generate the xml configuration to generate a link of this configuration. 
     */
    public String getXml(){
        String mb = getSlaveMobileBase()==null?"":"\n\t\t"+getSlaveMobileBase().getEmbedableXml() +"\n";
        return "\n\t<DHParameters>\n"+
            "\t\t<Delta>"+d+"</Delta>\n"+
            "\t\t<Theta>"+Math.toDegrees(theta)+"</Theta>\n"+
           "\t\t<Radius>"+radius+"</Radius>\n"+
           "\t\t<Alpha>"+Math.toDegrees(alpha)+"</Alpha>\n"+
           mb+
        "\t</DHParameters>\n";
    }
    
    public double getD() {
        return getDelta();
    }
 
    public double getTheta() {
        return theta;
    }
 
    public double getR() {
        return getRadius();
    }
 
    public double getAlpha() {
        return alpha;
    }
    
 
    public Matrix DhStepInverse(Matrix end, double jointValue) {    
        switch(type){
        case PRISMATIC:
            return DhStepInverse(end,0,jointValue);
        case ROTORY:
            return  DhStepInverse(end,jointValue,0);
        default:
        case TOOL:
            return  DhStepInverse(end,0,0);
        }
 
    }
    
    public Matrix DhStep(double jointValue) {
        switch(type){
        case PRISMATIC:
            return DhStep(0,jointValue);
        case ROTORY:
            return DhStep(jointValue,0);
        default:
        case TOOL:
            return DhStep(0,0);
        }
    }
    
    
    public Matrix DhStep(double rotory,double prismatic) {
 
        setMatrix(rotory, prismatic);
        
        Matrix step = getTransZ();
        step = step.times(getRotZ());
        step = step.times(getTransX());
        step = step.times(getRotX());
        
 
        return step;
    }
    
    public Matrix DhStepInverse(Matrix end,double rotory,double prismatic) {
        setMatrix(rotory, prismatic);
        
        Matrix step = end.times(getRotX().inverse());
        step = step.times(getTransX().inverse());
        step = step.times(getRotZ().inverse());
        step = step.times(getTransZ().inverse());
        
        return step;
    }
    
    public Matrix DhStepInverse(double rotory,double prismatic) {
        
        Matrix end=new TransformNR().getMatrixTransform();
        
        return DhStepInverse(end,rotory,prismatic);
    }
 
    
    public Matrix DhStepInversePrismatic(double prismatic) {
        
        return DhStepInverse(0,prismatic);
    }
 
    
    public Matrix DhStepInverseRotory(double rotory) {
        return DhStepInverse(rotory,0);
    }
 
 
    public void setTransX(Matrix transX) {
        this.transX = transX;
    }
 
    public void setRotX(Matrix rotX) {
        this.rotX = rotX;
    }
    
    
    private void setMatrix(double rotory,double prismatic){
        transZ = new Matrix( new double [][] {  
                {1,0,0,0},
                {0,1,0,0},
                {0,0,1,getD()+prismatic},
                {0,0,0,1}});
        
        rotZ=new Matrix( new double [][] {  
                {Math.cos(getTheta()+rotory),    -Math.sin(getTheta()+rotory),  0,  0},
                {Math.sin(getTheta()+rotory),     Math.cos(getTheta()+rotory),  0,  0},
                {0,                                                         0,  1,  0},
                {0,                                                         0,  0,  1}});
    }
    
    public Matrix getTransX() {
         if(transX == null){
             transX=new Matrix( new double [][] {   
                    {1,0,0,getR()},
                    {0,1,0,0},
                    {0,0,1,0},
                    {0,0,0,1}});
        }   
        return transX;
    }
 
    public Matrix getRotX() {
         if(rotX == null){
             rotX=new Matrix( new double [][] { 
                {1, 0,                      0,                          0},
                {0, Math.cos(getAlpha()),   -Math.sin(getAlpha()),      0},
                {0, Math.sin(getAlpha()),   Math.cos(getAlpha()),       0},
                {0, 0,                      0,                          1}});
         }
        return rotX;
    }
    
    public Matrix getTransZ() {
        return transZ;
    }
 
    public Matrix getRotZ() {
        return rotZ;
    }
    
    public Matrix DhStepJacobian(double rotoryVelocity,double prismaticVelocity) {
 
        setJacobianMatrix(rotoryVelocity, prismaticVelocity);
        
        Matrix step = getTransZ_J();
        step = step.times(getRotZ_J());
        step = step.times(getTransX_J());
        step = step.times(getRotX_J());
        
        return step;
    }
    
    private void setJacobianMatrix(double rotory,double prismatic){
        rotZ_J = new Matrix( new double [][] {  
                {1,0,0,                 0   },
                {0,1,0,                 0   },
                {0,0,1, getD()+prismatic    },
                {0,0,0,                 1   }});
        
        transZ_J  = new Matrix( new double [][] {   
                {Math.cos(getTheta()+rotory),-Math.sin(getTheta()+rotory),  0,  0},
                {Math.sin(getTheta()+rotory), Math.cos(getTheta()+rotory),  0,  0},
                {0,                                                     0,  1,  0},
                {0,                                                     0,  0,  1}});
    }
    
    public Matrix getRotX_J() {
         if(rotX_J == null){
             rotX_J = new Matrix( new double [][] { 
                {1, 0,                      0,                          0},
                {0, Math.cos(getAlpha()),   -Math.sin(getAlpha()),      0},
                {0, Math.sin(getAlpha()),   Math.cos(getAlpha()),       0},
                {0, 0,                      0,                          1}});
         }
        return rotX_J;
    }
    
    public Matrix getTransX_J() {
         if(transX_J == null){
             transX_J= new Matrix( new double [][] {    
                    {1,0,0,getR()},
                    {0,1,0,0},
                    {0,0,1,0},
                    {0,0,0,1}});
         }  
        return transX_J;
    }
 
 
 
    public Matrix getTransZ_J() {
        return transZ_J;
    }
    
    public Matrix getRotZ_J() {
        return rotZ_J;
    }
    
    /* (non-Javadoc)
     * @see java.lang.Object#toString()
     */
    @Override 
    public String toString(){
        String s="";
        s+=" Delta = "+getDelta();
        s+=" Theta = "+Math.toDegrees(getTheta())+" deg";
        s+=" Radius = "+getRadius();
        s+=" Alpha = "+Math.toDegrees(getAlpha())+" deg";
        return s;
    }
 
    public Object getListener() {
        return listener;
    }
 
    public void setListener(Object listener) {
        this.listener = listener;
    }
    
    public Object getRootListener() {
        return root;
    }
 
    void setRootListener(Object listener) {
        this.root = listener;
    }
    
    public double getDelta() {
        return d;
    }
 
    public void setDelta(double d) {
        this.d = d;
        if(newLinkConf!=null)newLinkConf.fireChangeEvent();
    }
 
    public double getRadius() {
        return radius;
    }
 
    public void setRadius(double radius) {
        this.radius = radius;
        transX_J=null;
        transX=null;
        if(newLinkConf!=null)newLinkConf.fireChangeEvent();
    }
 
    public void setTheta(double theta) {
        this.theta = theta;
        if(newLinkConf!=null)newLinkConf.fireChangeEvent();
    }
 
    public void setAlpha(double alpha) {
        this.alpha = alpha;
        rotX=null;
        rotX_J=null;
        if(newLinkConf!=null)newLinkConf.fireChangeEvent();
    }
 
    public DhLinkType getLinkType() {
        return type;
    }
 
    public void setLinkType(DhLinkType type) {
        this.type = type;
    }
 
    public void setMobileBaseXml(MobileBase embedableXml) {
        this.setSlaveMobileBase(embedableXml);
    }
 
    public MobileBase getSlaveMobileBase() {
        return slaveMobileBase;
    }
 
    public void setSlaveMobileBase(MobileBase embedableXml) {
        this.slaveMobileBase = embedableXml;
    }
 
}