com.neuronrobotics.sdk.addons.kinematics.DHParameterKinematics Maven / Gradle / Ivy

Go to download
Show more of this group Show more artifacts with this name
Show all versions of java-bowler Show documentation
A command line utility for accesing the bowler framework.
The newest version!
package com.neuronrobotics.sdk.addons.kinematics;

import java.io.File;
import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.InputStream;
import java.net.URL;
//import java.net.URL;
import java.nio.file.Files;
import java.nio.file.StandardCopyOption;
import java.util.ArrayList;

import org.w3c.dom.Element;

import javafx.application.Platform;
import javafx.scene.transform.Affine;
import Jama.Matrix;

import com.neuronrobotics.sdk.addons.kinematics.TransformFactory;
import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR;
import com.neuronrobotics.sdk.addons.kinematics.xml.XmlFactory;
import com.neuronrobotics.sdk.common.BowlerAbstractConnection;
import com.neuronrobotics.sdk.common.BowlerAbstractDevice;
import com.neuronrobotics.sdk.common.DeviceManager;
import com.neuronrobotics.sdk.common.IConnectionEventListener;
import com.neuronrobotics.sdk.common.IDeviceConnectionEventListener;
import com.neuronrobotics.sdk.common.Log;
import com.neuronrobotics.sdk.dyio.DyIO;
import com.neuronrobotics.sdk.pid.GenericPIDDevice;
import com.neuronrobotics.sdk.pid.VirtualGenericPIDDevice;

// TODO: Auto-generated Javadoc
/**
 * The Class DHParameterKinematics.
 */
public class DHParameterKinematics extends AbstractKinematicsNR
		implements ITaskSpaceUpdateListenerNR, IJointSpaceUpdateListenerNR {

	/** The chain. */
	private DHChain chain = null;

	/** The links listeners. */
	private ArrayList linksListeners = new ArrayList();

	/** The current target. */
	private Affine currentTarget = new Affine();

	/** The disconnecting. */
	boolean disconnecting = false;

	/** The l. */
	IDeviceConnectionEventListener l = new IDeviceConnectionEventListener() {
		@Override
		public void onDisconnect(BowlerAbstractDevice source) {
			if (!disconnecting) {
				disconnecting = true;
				//disconnect();
			}

		}

		@Override
		public void onConnect(BowlerAbstractDevice source) {
		}
	};

	private ArrayList configs;

	/**
	 * Instantiates a new DH parameter kinematics.
	 *
	 * @param bad
	 *            the bad
	 * @param linkStream
	 *            the link stream
	 */
	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;
			}
	}

	/**
	 * Instantiates a new DH parameter kinematics.
	 *
	 * @param bad
	 *            the bad
	 * @param linkStream
	 *            the link stream
	 */
	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;
			}
	}

	/**
	 * Instantiates a new DH parameter kinematics.
	 *
	 * @param bad
	 *            the bad
	 * @param linkStream
	 *            the link stream
	 * @param depricated
	 *            the depricated
	 */
	@Deprecated
	public DHParameterKinematics(BowlerAbstractDevice bad, InputStream linkStream, InputStream depricated) {
		this(bad, linkStream);
	}

	/**
	 * Instantiates a new DH parameter kinematics.
	 *
	 * @param bad
	 *            the bad
	 */
	public DHParameterKinematics(BowlerAbstractDevice bad) {
		this(bad, XmlFactory.getDefaultConfigurationStream("TrobotLinks.xml"));
	}

	/**
	 * Instantiates a new DH parameter kinematics.
	 *
	 * @param bad
	 *            the bad
	 * @param file
	 *            the file
	 */
	public DHParameterKinematics(BowlerAbstractDevice bad, String file) {
		this(bad, XmlFactory.getDefaultConfigurationStream(file));
	}

	/**
	 * Instantiates a new DH parameter kinematics.
	 *
	 * @param bad
	 *            the bad
	 * @param configFile
	 *            the config file
	 * @throws FileNotFoundException
	 *             the file not found exception
	 */
	public DHParameterKinematics(BowlerAbstractDevice bad, File configFile) throws FileNotFoundException {
		this(bad, new FileInputStream(configFile));
	}

	/**
	 * Instantiates a new DH parameter kinematics.
	 */
	public DHParameterKinematics() {
		this(null, XmlFactory.getDefaultConfigurationStream("TrobotLinks.xml"));
	}

	/**
	 * Instantiates a new DH parameter kinematics.
	 *
	 * @param file
	 *            the file
	 */
	public DHParameterKinematics(String file) {
		this(null, XmlFactory.getDefaultConfigurationStream(file));
	}

	/**
	 * Instantiates a new DH parameter kinematics.
	 *
	 * @param linkStream
	 *            the link stream
	 */
	public DHParameterKinematics(Element linkStream) {
		this(null, linkStream);
	}

	/**
	 * Instantiates a new DH parameter kinematics.
	 *
	 * @param configFile
	 *            the config file
	 * @throws FileNotFoundException
	 *             the file not found exception
	 */
	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;
	}
	   /**
     * Cross product.
     *
     * @param a the a
     * @param b the b
     * @return the double[]
     */
    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;
    }
        /**
     * Gets the Jacobian matrix.
     *
     * @param jointSpaceVector the joint space vector
     * @return a matrix representing the Jacobian for the current configuration
     */
    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;iindex) continue;
            Matrix rotationComponent = forwardOffset(new TransformNR()).getMatrixTransform();
            for(int j=i;j getChainTransformations() {
		return getChain().getChain(getCurrentJointSpaceVector());
	}

	/**
	 * Sets the dh chain.
	 *
	 * @param chain
	 *            the new dh chain
	 */
	public void setDhChain(DHChain chain) {
		this.setChain(chain);
	}

	/**
	 * Gets the dh chain.
	 *
	 * @return the dh chain
	 */
	public DHChain getDhChain() {
		return getChain();
	}

	/**
	 * Gets the chain.
	 *
	 * @return the chain
	 */
	public DHChain getChain() {
		return chain;
	}

	/**
	 * Sets the chain.
	 *
	 * @param chain
	 *            the new chain
	 */
	public void setChain(DHChain chain) {
		this.chain = chain;
		ArrayList dhLinks = chain.getLinks();
		for (int i = linksListeners.size(); i < dhLinks.size(); i++) {
			linksListeners.add(new Affine());
		}
		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);
		try {
			currentJointSpacePositions = null;
			currentJointSpaceTarget = null;
			// setDesiredJointSpaceVector(getCurrentJointSpaceVector(), 0);
		} catch (Exception e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}

	/*
	 * (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 = "\n";
		xml += getEmbedableXml();
		xml += "\n";
		return xml;
	}

	/**
	 * Gets the embedable xml.
	 *
	 * @return the embedable xml
	 */
	/*
	 * 
	 * Generate the xml configuration to generate an XML of this robot.
	 */
	public String getEmbedableXml() {

		String xml = "";

		xml += "\t\n";
		xml += "\t\t" + getGitCadEngine()[0] + "\n";
		xml += "\t\t" + getGitCadEngine()[1] + "\n";
		xml += "\t\n";

		xml += "\t\n";
		xml += "\t\t" + getGitDhEngine()[0] + "\n";
		xml += "\t\t" + getGitDhEngine()[1] + "\n";
		xml += "\t\n";

		ArrayList dhLinks = chain.getLinks();
		for (int i = 0; i < dhLinks.size(); i++) {
			xml += "\n";
			xml += getLinkConfiguration(i).getXml();
			xml += dhLinks.get(i).getXml();
			xml += "\n\n";
		}
		xml += "\n";
		xml += getFiducialToGlobalTransform().getXml();
		xml += "\n\n";

		xml += "\n\n";
		xml += getRobotToFiducialTransform().getXml();
		xml += "\n\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, getCurrentTargetAffine());
	}

	/**
	 * Gets the inverse solver.
	 *
	 * @return the inverse solver
	 */
	public DhInverseSolver getInverseSolver() {
		return chain.getInverseSolver();
	}

	/**
	 * Sets the inverse solver.
	 *
	 * @param inverseSolver
	 *            the new inverse solver
	 */
	public void setInverseSolver(DhInverseSolver inverseSolver) {
		chain.setInverseSolver(inverseSolver);
	}

	/**
	 * Gets the current target affine.
	 *
	 * @return the current target affine
	 */
	public Affine getCurrentTargetAffine() {
		return currentTarget;
	}

	/**
	 * Adds the new link.
	 *
	 * @param newLink
	 *            the new link
	 * @param dhLink
	 *            the dh link
	 */
	public void addNewLink(LinkConfiguration newLink, DHLink dhLink) {
		LinkFactory factory = getFactory();
		// remove the link listener while the number of links could chnage
		factory.removeLinkListener(this);
		factory.getLink(newLink);// adds new link internally
		DHChain chain = getDhChain();
		chain.addLink(dhLink);
		// set the modified kinematics chain
		setChain(chain);
		// once the new link configuration is set up, re add the listener
		factory.addLinkListener(this);
	}

	/**
	 * Removes the link.
	 *
	 * @param index
	 *            the index
	 */
	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);
	}

	/**
	 * Update cad locations.
	 */
	public void updateCadLocations() {
		synchronized (DHParameterKinematics.class) {
			try {
				ArrayList ll = getChain().getChain(getCurrentJointSpaceVector());
				for (int i = 0; i < ll.size(); i++) {
					ArrayList linkPos = ll;
					int index = i;
					Affine af = getChain().getLinks().get(index).getListener();
					TransformNR nr = linkPos.get(index);
					Platform.runLater(() -> {
						if (nr == null || af == null) {
							return;
						}
						try {
							TransformFactory.nrToAffine(nr, af);
						} catch (Exception ex) {
							// ex.printStackTrace();
						}
					});
				}
			} catch (Exception ex) {
				// ex.printStackTrace();
			}
		}
	}

	/*
	 * (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) {
		updateCadLocations();
	}

	/**
	 * Sets the global to fiducial transform.
	 *
	 * @param frameToBase
	 *            the new global to fiducial transform
	 */
	@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
			getChain().getChain(getCurrentJointSpaceVector());//calculate new locations
		}
		updateCadLocations();
	}

	/*
	 * (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 Affine getLinkAffineManipulator(int index) {
		 return getChain().getLinks().get(index).getListener();
	}
	/**
	 * Gets the theta.
	 *
	 * @return the theta
	 */

	public double getDH_Theta(int index) {
		return getChain().getLinks().get(index).getTheta();
	}
	/**
	 * Gets the d.
	 *
	 * @return the d
	 */
	public double getDH_D(int index) {
		return getChain().getLinks().get(index).getDelta();
	}



	/**
	 * Gets the r.
	 *
	 * @return the r
	 */
	public double getDH_R(int index) {
		return getChain().getLinks().get(index).getRadius();
	}

	/**
	 * Gets the alpha.
	 *
	 * @return the alpha
	 */
	public double getDH_Alpha(int index) {
		return getChain().getLinks().get(index).getAlpha();
	}
	
	/**
	 * Gets the theta.
	 *
	 * 
	 */

	public void setDH_Theta(int index, double value) {
		 getChain().getLinks().get(index).setTheta(value);
	}
	/**
	 * Gets the d.
	 *
	 * 
	 */
	public void setDH_D(int index, double value) {
		 getChain().getLinks().get(index).setDelta(value);
	}



	/**
	 * Gets the r.
	 *
	 * 
	 */
	public void setDH_R(int index, double value) {
		 getChain().getLinks().get(index).setRadius(value);
	}

	/**
	 * Gets the alpha.
	 *
	 * 
	 */
	public void setDH_Alpha(int index, double value) {
		 getChain().getLinks().get(index).setAlpha(value);
	}
}