An example of making custom kinematics implementations

customdh.groovy

import com.neuronrobotics.sdk.common.BowlerAbstractDevice
import com.neuronrobotics.sdk.common.DeviceManager;
import com.neuronrobotics.sdk.common.NonBowlerDevice
import com.neuronrobotics.sdk.dyio.DyIO
import com.neuronrobotics.sdk.dyio.peripherals.DigitalInputChannel
import com.neuronrobotics.sdk.dyio.peripherals.DigitalOutputChannel
import com.neuronrobotics.sdk.dyio.peripherals.IDigitalInputListener;
import com.neuronrobotics.sdk.dyio.peripherals.ServoChannel
import com.neuronrobotics.sdk.namespace.bcs.pid.IPidControlNamespace;
import com.neuronrobotics.sdk.pid.PIDChannel; // Needed for PID.
import com.neuronrobotics.sdk.pid.PIDConfiguration;
import com.neuronrobotics.bowlerstudio.robots.FormacarumRover
import com.neuronrobotics.bowlerstudio.robots.ObjectDetectionDataTableElement
import com.neuronrobotics.bowlerstudio.robots.RoverDataTable
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.ServoRotoryLink;
import com.neuronrobotics.sdk.addons.kinematics.math.RotationNR
import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR;
import com.neuronrobotics.sdk.pid.VirtualGenericPIDDevice;
import com.neuronrobotics.sdk.ui.ConnectionDialog;
import com.neuronrobotics.sdk.util.ThreadUtil;

import eu.mihosoft.vrl.v3d.CSG;
import eu.mihosoft.vrl.v3d.Cube;
import eu.mihosoft.vrl.v3d.Sphere;
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 java.util.ArrayList;

import javafx.application.Platform;
import javafx.scene.paint.Color;

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

boolean useVirtual = false;
//useVirtual=true;

public class ComputedModel  implements DhInverseSolver{
	private DHChain dhChain;
	public ComputedModel(DHChain dhChain){
		this.dhChain=dhChain;
	}
	private static final double M_PI = Math.PI;
	public double[] inverseKinematics(TransformNR target,double[] jointSpaceVector, DHChain chain  ) {

		int linkNum = jointSpaceVector.length;
		double [] inv = new double[linkNum];
		// this is an ad-hock kinematic model for d-h parameters and only works for specific configurations

		double dx = dhChain.getLinks().get(1).getD()-
				dhChain.getLinks().get(2).getD();
		double dy = dhChain.getLinks().get(0).getR();
		
		double xSet = target.getX();
		double ySet = target.getY();
		
		double polarR = Math.sqrt(xSet*xSet+ySet*ySet);
		double polarTheta = Math.asin(ySet/polarR);
		
		
		double adjustedR = Math.sqrt((polarR*polarR)+(dx*dx))-dy;
		double adjustedTheta =Math.asin(dx/polarR);
		
		
		xSet = adjustedR*Math.sin(polarTheta-adjustedTheta);
		ySet = adjustedR*Math.cos(polarTheta-adjustedTheta);
	
		
		double orentation = polarTheta-adjustedTheta;
		
		double zSet = target.getZ() +
				dhChain.getLinks().get(4).getD()-
				dhChain.getLinks().get(0).getD();
		// Actual target for anylitical solution is above the target minus the z offset
		TransformNR overGripper = new TransformNR(
				xSet,
				ySet,
				zSet,
				target.getRotation());


		double l1 = dhChain.getLinks().get(1).getR();// First link length
		double l2 = dhChain.getLinks().get(2).getR();

		double vect = Math.sqrt(xSet*xSet+ySet*ySet+zSet*zSet)
		System.out.println("TO: "+overGripper);
		System.out.println("polarR: "+polarR);
		System.out.println("polarTheta: "+Math.toDegrees(polarTheta));
		System.out.println("adjustedTheta: "+Math.toDegrees(adjustedTheta));
		System.out.println("adjustedR: "+adjustedR);
		
		System.out.println("x Correction: "+xSet);
		System.out.println("y Correction: "+ySet);
		
		System.out.println("Orentation: "+Math.toDegrees(orentation));
		System.out.println("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);


		System.out.println("vect: "+vect);
		System.out.println("A: "+Math.toDegrees(A));
		System.out.println("elevation: "+Math.toDegrees(elevation));
		System.out.println("l1 from x/y plane: "+Math.toDegrees(A+elevation));
		System.out.println("l2 from l1: "+Math.toDegrees(C));
		inv[0] = Math.toDegrees(orentation);
		inv[1] = -Math.toDegrees((A+elevation+dhChain.getLinks().get(1).getTheta()));
		inv[2] = (Math.toDegrees(C))+//interior angle of the triangle, map to external angle
				Math.toDegrees(dhChain.getLinks().get(2).getTheta());// offset for kinematics
		inv[3] =(inv[1] -inv[2]);// keep it parallell
		// We know the wrist twist will always be 0 for this model
		inv[4] = inv[0];//keep the camera orentation paralell from the base
		for(int i=0;i<inv.length;i++){
			println "Link#"+i+" is set to "+inv[i];
		}
		//copy over remaining links so they do not move
		for(int i=6;i<inv.length && i<jointSpaceVector.length ;i++){
			inv[i]=jointSpaceVector[i];
		}

		return inv;
	}
}

System.err.println("Starting Model")
//Create the model
MobileBase base=null;
if(DeviceManager.getSpecificDevice(MobileBase.class, "TrobotArmGroup")==null){
	String xmlContent;
	xmlContent = ScriptingEngine.codeFromGistID("0e6454891a3b3f7c8f28","trobotServo.xml")[0];
	//Or use a local file
	if(xmlContent==null)
		xmlContent=new String(Files.readAllBytes(Paths.get(System.getProperty("user.home")+"/bowler-workspace/trobotServo.xml")));
	BowlerStudio.speak("I did not fine a device called TrobotArmGroup. Connecting TrobotArmGroup.");
	base = new MobileBase(IOUtils.toInputStream(xmlContent, "UTF-8"));
	DeviceManager.addConnection(base,base.getScriptingName());
}else{
	base = (MobileBase)DeviceManager.getSpecificDevice(MobileBase.class, "TrobotArmGroup");
}

DHParameterKinematics DHArm = base.getAppendages().get(0);

DHArm.setInverseSolver(new ComputedModel(DHArm.getDhChain()));

trobotServo.xml

<root>
<mobilebase>

<driveType>none</driveType>
	<cadEngine>
		<gist>bcb4760a449190206170</gist>
		<file>ThreeDPrintCad.groovy</file>
	</cadEngine>
	<driveEngine>
		<gist>bcb4760a449190206170</gist>
		<file>WalkingDriveEngine.groovy</file>
	</driveEngine>

<name>TrobotArmGroup</name>
<appendage>

<name>TrobotArm</name>
	<cadEngine>
		<gist>bcb4760a449190206170</gist>
		<file>ThreeDPrintCad.groovy</file>
	</cadEngine>
	<kinematics>
		<gist>bcb4760a449190206170</gist>
		<file>DefaultDhSolver.groovy</file>
	</kinematics>
<link>
	<name>basePan</name>
	<deviceName>dyio</deviceName>
	<type>servo-rotory</type>
	<index>11</index>
	<scale>0.33</scale>
	<upperLimit>255.0</upperLimit>
	<lowerLimit>0.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>235.0</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>235</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

	<DHParameters>
		<Delta>13.0</Delta>
		<Theta>0.0</Theta>
		<Radius>32.0</Radius>
		<Alpha>-90.0</Alpha>
	</DHParameters>

</link>
<link>
	<name>baseTilt</name>
	<deviceName>dyio</deviceName>
	<type>servo-rotory</type>
	<index>10</index>
	<scale>0.33</scale>
	<upperLimit>255.0</upperLimit>
	<lowerLimit>0.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>128.0</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>128</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

	<DHParameters>
		<Delta>25.0</Delta>
		<Theta>-90.0</Theta>
		<Radius>93.0</Radius>
		<Alpha>180.0</Alpha>
	</DHParameters>

</link>
<link>
	<name>elbow</name>
	<deviceName>dyio</deviceName>
	<type>servo-rotory</type>
	<index>9</index>
	<scale>0.33</scale>
	<upperLimit>255.0</upperLimit>
	<lowerLimit>0.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>121.0</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>121</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

	<DHParameters>
		<Delta>11.0</Delta>
		<Theta>-90.0</Theta>
		<Radius>131.0</Radius>
		<Alpha>0.0</Alpha>
	</DHParameters>

</link>
<link>
	<name>wrist2</name>
	<deviceName>dyio</deviceName>
	<type>servo-rotory</type>
	<index>7</index>
	<scale>-0.313</scale>
	<upperLimit>255.0</upperLimit>
	<lowerLimit>0.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>200.0</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>175</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

	<DHParameters>
		<Delta>0.0</Delta>
		<Theta>0.0</Theta>
		<Radius>0.0</Radius>
		<Alpha>90.0</Alpha>
	</DHParameters>

</link>
<link>
	<name>wrist3</name>
	<deviceName>dyio</deviceName>
	<type>servo-rotory</type>
	<index>6</index>
	<scale>0.3125</scale>
	<upperLimit>255.0</upperLimit>
	<lowerLimit>0.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>255.0</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>255</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

	<DHParameters>
		<Delta>98.0</Delta>
		<Theta>0.0</Theta>
		<Radius>0.0</Radius>
		<Alpha>-90.0</Alpha>
	</DHParameters>

</link>

<ZframeToRAS
>	<x>0.0</x>
	<y>0.0</y>
	<z>0.0</z>
	<rotw>1.0</rotw>
	<rotx>0.0</rotx>
	<roty>0.0</roty>
	<rotz>0.0</rotz>
</ZframeToRAS>

<baseToZframe>
	<x>0.0</x>
	<y>0.0</y>
	<z>0.0</z>
	<rotw>1.0</rotw>
	<rotx>0.0</rotx>
	<roty>0.0</roty>
	<rotz>0.0</rotz>
</baseToZframe>

</appendage>

<ZframeToRAS>
	<x>0.0</x>
	<y>0.0</y>
	<z>0.0</z>
	<rotw>1.0</rotw>
	<rotx>0.0</rotx>
	<roty>0.0</roty>
	<rotz>0.0</rotz>
</ZframeToRAS>

<baseToZframe>
	<x>0.0</x>
	<y>0.0</y>
	<z>0.0</z>
	<rotw>1.0</rotw>
	<rotx>0.0</rotx>
	<roty>0.0</roty>
	<rotz>0.0</rotz>
</baseToZframe>

</mobilebase>

</root>