helper functions

Author: madhephaestus

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

@@ -113,6 +113,13 @@ public abstract class AbstractKinematicsNR extends NonBowlerDevice implements IP
 	private IMU imu = new IMU();
 
 	private Runnable renderWrangler=null;
+	public int getLinkIndex(AbstractLink l) {
+		for (int i=0;i<getNumberOfLinks();i++) {
+			if(getAbstractLink(i)==l)
+				return i;
+		}
+		return -1;
+	}
 
 	/**
 	 * Gets the root listener.

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

@@ -93,7 +93,13 @@ public HashMap<DHParameterKinematics, TransformNR> getTipLocations() {
 		}
 		return tipList;
 	}
-
+	public DHParameterKinematics getLimb(AbstractLink l) {
+		for(DHParameterKinematics k:getAllDHChains()) {
+			if(k.getLinkIndex(l)>=0)
+				return k;
+		}
+		return null;
+	}
 	public boolean pose(TransformNR newAbsolutePose) throws Exception {
 		HashMap<DHParameterKinematics, TransformNR> tipLocations = getTipLocations();