customdh.groovy

import com.neuronrobotics.bowlerstudio.scripting.ScriptingEngineWidget;
import com.neuronrobotics.sdk.addons.kinematics.DHChain;
import com.neuronrobotics.sdk.addons.kinematics.DHLink;
import com.neuronrobotics.sdk.addons.kinematics.DHParameterKinematics;
import com.neuronrobotics.sdk.addons.kinematics.DhInverseSolver
import com.neuronrobotics.sdk.addons.kinematics.math.RotationNR
import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR;
import com.neuronrobotics.sdk.pid.VirtualGenericPIDDevice;

import eu.mihosoft.vrl.v3d.CSG;
import eu.mihosoft.vrl.v3d.Cube;
import eu.mihosoft.vrl.v3d.Transform;

import java.nio.file.Files;
import java.nio.file.Paths;
import java.nio.file.StandardCopyOption;
import java.time.Duration;

import javafx.application.Platform;

import org.apache.commons.io.IOUtils;
import org.reactfx.util.FxTimer;

public class ComputedGeometricModel  implements DhInverseSolver{
	private DHChain dhChain;
	private static final double M_PI = Math.PI;
	public ComputedGeometricModel(DHChain dhChain) {
		this.dhChain = dhChain;
	}
	private double sqrt(double d) {
		return Math.sqrt(d);
	}
	private double atan2(double y, double x) {
		return Math.atan2(y, x);
	}
	private double acos(double d) {
		return Math.acos(d);
	}
	private double sin(double angle) {
		return Math.sin(angle);
	}
	private double cos(double angle) {
		return Math.cos(angle);
	}
	private double ToRadians(double degrees){
		return degrees*Math.PI/180.0;
	}
	private double ToDegrees(double rad){
		return rad*180.0/Math.PI;
	}
	public double[] inverseKinematics(TransformNR target,double[] jointSpaceVector ) {
		viewer.addTransform(target, "Target",Color.pink);
				int linkNum = jointSpaceVector.length;
		double [] inv = new double[linkNum];
		 // this is an ad-hock kinematic model
		 // We know the wrist twist will always be 0 for this model
		 inv[4] = 0;
		
		 // Actual target for anylitical solution is above the target minus the z offset
		 TransformNR overGripper = new TransformNR(target.getX(),
		 					target.getY(),
		 					target.getZ() + 
		 						dhChain.getLinks().get(4).getR()-
		 						dhChain.getLinks().get(0).getD()
		 						,
		 					new RotationNR());
		 double l1 = dhChain.getLinks().get(1).getR();// First link length
		 double l2 = Math.sqrt(	Math.pow(dhChain.getLinks().get(2).getR(),2) + 
		 						Math.pow(168.25,2)
		 			);// Second link length as a 90 degree bend to it
		 double link2AddAngle = Math.atan2(	168.25,
		 									dhChain.getLinks().get(2).getR()											
		 									);
		 double xSet = overGripper.getX();
		 double ySet = overGripper.getY();
		 double zSet = overGripper.getZ();

		 double thetaLocal = Math.atan2(ySet, xSet);
		
		 double vect = sqrt(xSet*xSet+ySet*ySet+zSet*zSet);
		 double xYvect = sqrt(xSet*xSet+ySet*ySet);
		 System.out.println("Theta local: "+thetaLocal+", Links: ,"+l1+","+l2+" vector distance: "+vect+", z: "+zSet);
		
		 if (vect > l1+l2) {
		 	throw new RuntimeException("Hypotenus too long: "+vect+" longer then "+l1+l2);
		 }
		 //from https://www.mathsisfun.com/algebra/trig-solving-sss-triangles.html
		 double a=l2;
		 double b=l1;
		 double c=vect;
		 double A =Math.acos((Math.pow(b,2)+ Math.pow(c,2) - Math.pow(a,2)) / (2*b*c));
		 double B =Math.acos((Math.pow(c,2)+ Math.pow(a,2) - Math.pow(b,2)) / (2*a*c));
		 double C =Math.PI-A-B;//Rule of triangles
		 double elevation = Math.asin(zSet/vect);
		 double orentation = Math.asin(ySet/xYvect);
		
		 inv[0] = ToDegrees(orentation);
		 inv[1] = 90-ToDegrees(A+elevation);
		 inv[2] = 90-ToDegrees(C)-ToDegrees(link2AddAngle);
		 inv[3] = -inv[1] -inv[2];// keep it parallell

		return inv;
	}
}
//define the file to load
String[] xml = ScriptingEngineWidget.codeFromGistID("e0c6a0f68430ebd6838c","rover.xml");
String xmlContent = xml[0];
//Or use a local file
//String xmlContent=new String(Files.readAllBytes(Paths.get("/home/hephaestus/git/0e6454891a3b3f7c8f28/rover.xml")));

System.err.println("Starting Model")
//Create the model
DHParameterKinematics model = new DHParameterKinematics(new VirtualGenericPIDDevice(100000),
			IOUtils.toInputStream(xmlContent, "UTF-8"),
			IOUtils.toInputStream(xmlContent, "UTF-8"));
//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)
model.setDesiredJointAxisValue(0, 0.1, 0);
for(DHLink dh : model.getDhChain().getLinks() ){
	System.out.println("Link D-H values = "+dh);
	// Create an axis to represent the link
	double y = dh.d>0?dh.d:20;
	double  x= dh.r>0?dh.r:20;
	
	CSG cube = new Cube(x,y,20).noCenter().toCSG();
	cube=cube.transformed(new Transform().translateX(-x));
	//add listner to axis
	cube.setManipulator(dh.getListener());
	// add ax to list of objects to be returned
	links.add(cube);
}


 
return links;

rover.xml

<!--<link>-->
<!--    <name></name>-->
<!--    <type>pid</type>-->
<!--    <index>0</index>-->
<!--    <scale>.088</scale>-->
<!--    <upperLimit>4096</upperLimit>-->
<!--    <lowerLimit>-4096</lowerLimit>-->
<!--    <isLatch>true</isLatch>-->
<!--    <indexLatch>0</indexLatch>-->
<!--    <isStopOnLatch>false</isStopOnLatch>-->
<!--    <homingTPS>500</homingTPS>-->
<!--    <DHParameters>-->
<!--      <Delta></Delta>-->
<!--      <Theta></Theta>-->
<!--      <Radius></Radius>-->
<!--      <Alpha></Alpha>-->
<!--    </DHParameters>-->
<!--  </link>-->

<root>
  <link>
    <name>RotToFirst</name>
    <type>pid</type>
    <index>0</index>
    <scale>.088</scale>
    <upperLimit>4096</upperLimit>
    <lowerLimit>-4096</lowerLimit>
    <isLatch>true</isLatch>
    <indexLatch>0</indexLatch>
    <isStopOnLatch>false</isStopOnLatch>
    <homingTPS>500</homingTPS>
    <DHParameters>
      <Delta>106.36</Delta>
      <Theta>180</Theta>
      <Radius>12.7</Radius>
      <Alpha>90</Alpha>
    </DHParameters>
  </link>
  <link>
    <name>FirstToSecond</name>
    <type>pid</type>
    <index>1</index>
    <scale>.088</scale>
    <upperLimit>4096</upperLimit>
    <lowerLimit>-4096</lowerLimit>
    <isLatch>true</isLatch>
    <indexLatch>0</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>SecondToBend</name>
    <type>pid</type>
    <index>3</index>
    <scale>.088</scale>
    <upperLimit>4096</upperLimit>
    <lowerLimit>-4096</lowerLimit>
    <isLatch>true</isLatch>
    <indexLatch>0</indexLatch>
    <isStopOnLatch>false</isStopOnLatch>
    <homingTPS>500</homingTPS>
    <DHParameters>
      <Delta>0</Delta>
      <Theta>111.9</Theta>
      <Radius>452</Radius>
      <Alpha>0</Alpha>
    </DHParameters>
  </link>
  <link>
    <name>BendToTwist</name>
    <type>pid</type>
    <index>4</index>
    <scale>.088</scale>
    <upperLimit>4096</upperLimit>
    <lowerLimit>-4096</lowerLimit>
    <isLatch>true</isLatch>
    <indexLatch>0</indexLatch>
    <isStopOnLatch>false</isStopOnLatch>
    <homingTPS>500</homingTPS>
    <DHParameters>
      <Delta>0</Delta>
      <Theta>158.1</Theta>
      <Radius>0</Radius>
      <Alpha>90</Alpha>
    </DHParameters>
  </link>
  <link>
    <name>TwistToCam</name>
    <type>pid</type>
    <index>5</index>
    <scale>.088</scale>
    <upperLimit>4096</upperLimit>
    <lowerLimit>-4096</lowerLimit>
    <isLatch>true</isLatch>
    <indexLatch>0</indexLatch>
    <isStopOnLatch>false</isStopOnLatch>
    <homingTPS>500</homingTPS>
    <DHParameters>
      <Delta>0</Delta>
      <Theta>0</Theta>
      <Radius>0</Radius>
      <Alpha>90</Alpha>
    </DHParameters>
  </link>
</root>

<!--
<root>
 comment
	<link>
		<name>RotationTurretPlate</name>
		<type>pid</type>
		<index>0</index>
		<scale>.088</scale>
		<upperLimit>4096</upperLimit>
		<lowerLimit>-4096</lowerLimit>
		<isLatch>true</isLatch>
		<indexLatch>0</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>4096</upperLimit>
		<lowerLimit>-4096</lowerLimit>
		<isLatch>true</isLatch>
		<indexLatch>0</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>secondTilt</name>
		<type>pid</type>
		<index>2</index>
		<scale>.088</scale>
		<upperLimit>4096</upperLimit>
		<lowerLimit>-4096</lowerLimit>
		<isLatch>true</isLatch>
		<indexLatch>0</indexLatch>
		<isStopOnLatch>false</isStopOnLatch>
		<homingTPS>500</homingTPS>
		<DHParameters>
		    <Delta>0</Delta>
		    <Theta>-111.9</Theta>
		    <Radius>452</Radius>
		    <Alpha>0</Alpha>
		</DHParameters>
	</link>
	<link>
		<name>wristTilt</name>
		<type>pid</type>
		<index>3</index>
		<scale>.756</scale>
		<upperLimit>4096</upperLimit>
		<lowerLimit>-4096</lowerLimit>
		<isLatch>true</isLatch>
		<indexLatch>0</indexLatch>
		<isStopOnLatch>false</isStopOnLatch>
		<homingTPS>500</homingTPS>
		<DHParameters>
		    <Delta>0</Delta>
		    <Theta>21.9</Theta>
		    <Radius>0</Radius>
		    <Alpha>90</Alpha>
		</DHParameters>	
	</link>
	<link>
		<name>wristTwist</name>
		<type>pid</type>
		<index>4</index>
		<scale>.756</scale>
		<upperLimit>4096</upperLimit>
		<lowerLimit>-4096</lowerLimit>
		<isLatch>true</isLatch>
		<indexLatch>0</indexLatch>
		<isStopOnLatch>false</isStopOnLatch>
		<homingTPS>500</homingTPS>
		<DHParameters>
		    <Delta>280</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>-->