com.neuronrobotics.sdk.addons.kinematics.SearchTreeSolver 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.util.ArrayList;

import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR;

// TODO: Auto-generated Javadoc
/**
 * The Class SearchTreeSolver.
 */
public class SearchTreeSolver implements DhInverseSolver {
	
	/** The dh chain. */
	private DHChain dhChain;
	
	/** The upper. */
	private double [] upper;
	
	/** The lower. */
	private double [] lower;
	
	/** The debug. */
	private boolean debug;
	
	/** The starting increment. */
	double startingIncrement = 1.5;//degrees
	
	/** The target. */
	private TransformNR target;
	
	/**
	 * Instantiates a new search tree solver.
	 *
	 * @param dhChain the dh chain
	 * @param debug the debug
	 */
	public SearchTreeSolver(DHChain dhChain, boolean debug) {
		this.setDhChain(dhChain);
		this.debug = debug;
		upper = dhChain.getUpperLimits();
		lower = dhChain.getlowerLimits();
	}

	/* (non-Javadoc)
	 * @see com.neuronrobotics.sdk.addons.kinematics.DhInverseSolver#inverseKinematics(com.neuronrobotics.sdk.addons.kinematics.math.TransformNR, double[], com.neuronrobotics.sdk.addons.kinematics.DHChain)
	 */
	@Override
	public double[] inverseKinematics(TransformNR target,double[] jointSpaceVector, 
			 DHChain chain ) {
		ArrayList links = chain.getLinks();
		setTarget(target);
		searchTree step=new searchTree(jointSpaceVector,startingIncrement);;
		boolean done = false;
		configuration conf = new configuration(jointSpaceVector, target);
//		double previousV =conf.getOffsetOrentationMagnitude();
//		double previousO =conf.getOffsetVectorMagnitude();
		int iter = 1000;
		int i = 0;
		do{
			double [] current = conf.getJoints();
			conf = step.getBest(current);
			
			double vect = conf.getOffsetOrentationMagnitude();
			double orent = conf.getOffsetVectorMagnitude();
			
			if(vect<10 && orent< .05){
				done = true;
				System.out.println("SearchTreeSolver Success stats: \n\tIterations = "+i+" out of "+iter+"\n"+conf);
			}
			if(i++==iter){
				done = true;
				System.err.println("SearchTreeSolver FAILED stats: \n\tIterations = "+i+" out of "+iter+"\n"+conf);
			}
		}while(! done);

		return conf.getJoints();
	}
	
	/**
	 * Gets the target.
	 *
	 * @return the target
	 */
	public TransformNR getTarget() {
		return target;
	}

	/**
	 * Sets the target.
	 *
	 * @param target the new target
	 */
	public void setTarget(TransformNR target) {
		this.target = target;
	}

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

	/**
	 * Sets the dh chain.
	 *
	 * @param dhChain the new dh chain
	 */
	public void setDhChain(DHChain dhChain) {
		this.dhChain = dhChain;
	}
	
	/**
	 * Fk.
	 *
	 * @param jointSpaceVector the joint space vector
	 * @return the transform nr
	 */
	public TransformNR fk(double[] jointSpaceVector){
		return getDhChain().forwardKinematics(jointSpaceVector);
	}

	/**
	 * The Class searchTree.
	 */
	private class searchTree{
		
		/** The nodes. */
		//double[] start;
		searchNode [] nodes;
		
		/**
		 * Instantiates a new search tree.
		 *
		 * @param jointSpaceVector the joint space vector
		 * @param startingIncrement the starting increment
		 */
		public searchTree(double[] jointSpaceVector,double startingIncrement){
			nodes = new searchNode [jointSpaceVector.length];
			for(int i=0;i configurations = new ArrayList ();
			for(int i=0;igetJoints()[i]+.1 ||
						c.getJoints()[i]getDhChain().getUpperLimits()[link] ||
					bgetDhChain().getUpperLimits()[link] ||
					b