ensure that when requesting the chain of transforms that it always comes

back with valid data

Author: madhephaestus

src/main/java/com/neuronrobotics/sdk/addons/kinematics/AbstractKinematicsNR.java

@@ -1366,5 +1366,30 @@ protected String getCode(Element e,String tag){
 	public IMU getImu() {
 		return imu;
 	}
+	
+	//New helper functions to make the API simpler
+	
+	public void boundedLinkValueSet( int index,double value) throws Exception {
+		value=boundToLinkLimits(index,value);
+		double[] vect =getCurrentJointSpaceVector();
+		vect[index]=value;
+		setDesiredJointSpaceVector(vect, 0);
+	}
+	public double boundToLinkLimits( int index,double value) {
+		AbstractLink l1 = getAbstractLink(index);
+		if(value>l1.getMaxEngineeringUnits()){
+			value=l1.getMaxEngineeringUnits();
+		}
+		if(value<l1.getMinEngineeringUnits()){
+			value=l1.getMinEngineeringUnits();
+		}
+		return value;
+	}
+	
+	public double linkMass(int linkIndex) {
+		return getLinkConfiguration(linkIndex).getMassKg();
+	}
+
+	
 
 }

src/main/java/com/neuronrobotics/sdk/addons/kinematics/DHChain.java

@@ -186,14 +186,15 @@ public TransformNR forwardKinematics(double[] jointSpaceVector, boolean store) {
 	 * @return the matrix
 	 */
 	public Matrix forwardKinematicsMatrix(double[] jointSpaceVector, boolean store) {
+		return forwardKinematicsMatrix(jointSpaceVector,store?getCachedChain():null);
+	}
+	private 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");
 		Matrix current = new TransformNR().getMatrixTransform();
-		if(store)
-			setChain(new ArrayList<TransformNR>());
-		ArrayList<TransformNR> cachedChain = getCachedChain();
+		
 		for(int i=0;i<getLinks().size();i++) {
 			LinkConfiguration conf= getFactory().getLinkConfigurations().get(i);
 			Matrix step;
@@ -208,17 +209,17 @@ public Matrix forwardKinematicsMatrix(double[] jointSpaceVector, boolean store)
 			final TransformNR pose =forwardOffset(new TransformNR(update));
 			//getLinks().get(index).fireOnLinkGlobalPositionChange(pose);	
 
-			if(store){
+			if(chainToLoad!=null){
 				if(intChain.size()<=i)
 					intChain.add(new TransformNR(step));
 				else{
 					intChain.set(i, new TransformNR(step));
 				}
 
-				if(cachedChain.size()<=i)
-					cachedChain.add(pose);
+				if(chainToLoad.size()<=i)
+					chainToLoad.add(pose);
 				else{
-					cachedChain.set(i, pose);
+					chainToLoad.set(i, pose);
 				}
 			}
 		}
@@ -256,8 +257,9 @@ public void setChain(ArrayList<TransformNR> chain) {
 	 * @return the chain
 	 */
 	public ArrayList<TransformNR> getChain(double[] jointSpaceVector) {
-		forwardKinematics(jointSpaceVector,true);
-		return getCachedChain();
+		ArrayList<TransformNR> chainToLoad = new ArrayList<TransformNR>();
+		forwardKinematicsMatrix(jointSpaceVector,chainToLoad);
+		return chainToLoad;
 	}
 
 	/**