customdh.groovy

import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR;
import com.neuronrobotics.sdk.pid.VirtualGenericPIDDevice;
import java.nio.file.Files;
import java.nio.file.StandardCopyOption;
public class ComputedGeometricModel  implements DhInverseSolver{
	private DHChain dhChain;
	public ComputedGeometricModel(DHChain dhChain) {
		this.dhChain = dhChain;
	}
	public double[] inverseKinematics(TransformNR target,double[] jointSpaceVector ) {
		//viewer.addTransform(target, "Target",Color.pink);
				int linkNum = jointSpaceVector.length;
		double [] inv = new double[linkNum];
		//Attempting to implement:
		//http://www.ri.cmu.edu/pub_files/pub1/xu_yangsheng_1993_1/xu_yangsheng_1993_1.pdf
		TransformNR current = dhChain.forwardKinematics(jointSpaceVector);
		return inv;
	}
}
//Raw file of the XML below
URL website = new URL("https://gist.githubusercontent.com/madhephaestus/0e6454891a3b3f7c8f28/raw/6dbeb29658c5335fc5302bcea5b37e2a70ee1365/rover.xml");
File target= new File("rover.xml");
Files.copy(website.openStream(), target.toPath(), StandardCopyOption.REPLACE_EXISTING);
//Create the model
DHParameterKinematics model = new DHParameterKinematics(new VirtualGenericPIDDevice(100000),
			target);
//Add the custom inverse solver
model.setInverseSolver(new ComputedGeometricModel(model.getDhChain()));
//Return new model to UI
//Creating a list of objects, one for each link
ArrayList<Object> links = new ArrayList<Object>();
model.setScriptingName("DHArm")
links.add(model)
for(DHLink dh : model.getDhChain().getLinks() ){
	System.out.println("Link D-H values = "+dh);
	// Create an axis to represent the link
	CSG cube = new Cube(20).toCSG();
	//add listner to axis
	cube.setManipulator(dh.getListener());
	// add ax to list of objects to be returned
	links.add(cube);
}
 
return links;

rover.xml

<root>
	
	<link>
		<name>RotationTurretPlate</name>
		<type>pid</type>
		<index>0</index>
		<scale>.088</scale>
		<upperLimit>196</upperLimit>
		<lowerLimit>41</lowerLimit>
		<isLatch>true</isLatch>
		<indexLatch>189</indexLatch>
		<isStopOnLatch>false</isStopOnLatch>	
		<homingTPS>500</homingTPS>
		<DHParameters>
		    <Delta>106.5</Delta>
		    <Theta>0</Theta>
		    <Radius>-12.7</Radius>
		    <Alpha>-90</Alpha>
		</DHParameters>	
	</link>
	<link>
		<name>firstTilt</name>
		<type>pid</type>
		<index>1</index>
		<scale>.088</scale>
		<upperLimit>205</upperLimit>
		<lowerLimit>41</lowerLimit>
		<isLatch>true</isLatch>
		<indexLatch>90</indexLatch>
		<isStopOnLatch>false</isStopOnLatch>
		<homingTPS>500</homingTPS>
		<DHParameters>
		    <Delta>0</Delta>
		    <Theta>90</Theta>
		    <Radius>476.25</Radius>
		    <Alpha>0</Alpha>
		</DHParameters>			
	</link>
	
	<link>
		<name>wristTwist</name>
		<type>pid</type>
		<index>2</index>
		<scale>.088</scale>
		<upperLimit>205</upperLimit>
		<lowerLimit>41</lowerLimit>
		<isLatch>true</isLatch>
		<indexLatch>189</indexLatch>
		<isStopOnLatch>false</isStopOnLatch>
		<homingTPS>500</homingTPS>
		<DHParameters>
		    <Delta>0</Delta>
		    <Theta>-90</Theta>
		    <Radius>419</Radius>
		    <Alpha>90</Alpha>
		</DHParameters>				
	</link>
	
	<link>
		<name>wristBend</name>
		<type>pid</type>
		<index>3</index>
		<scale>.756</scale>
		<upperLimit>197</upperLimit>
		<lowerLimit>0</lowerLimit>
		<isLatch>true</isLatch>
		<indexLatch>111</indexLatch>
		<isStopOnLatch>false</isStopOnLatch>	
		<homingTPS>500</homingTPS>
		<DHParameters>
		    <Delta>-168.25</Delta>
		    <Theta>0</Theta>
		    <Radius>0</Radius>
		    <Alpha>-90</Alpha>
		</DHParameters>			
	</link>
	

	
	<baseToZframe>
		<x>0</x>
		<y>0</y>
		<z>0</z>
		<rotw>1</rotw>
		<rotx>0</rotx>
		<roty>0</roty>
		<rotz>0</rotz>
	</baseToZframe>
</root>