KickOff.groovy

import java.io.File;

import com.neuronrobotics.bowlerstudio.robots.FormacarumRover;
import com.neuronrobotics.bowlerstudio.scripting.ScriptingEngineWidget;
import com.neuronrobotics.sdk.common.DeviceManager;
import com.neuronrobotics.sdk.dyio.DyIO;
import com.neuronrobotics.sdk.util.ThreadUtil;

//run the rover initialization scripts, ignore the display returns
ScriptingEngineWidget.inlineFileScriptRun(new File(System.getProperty("user.home")+"/git/9bbfbcae11130cdd4c3d/RoverStartup.groovy"), null);
DyIO dyio =(DyIO)DeviceManager.getSpecificDevice(DyIO.class, "dyio");
FormacarumRover rover = (FormacarumRover)DeviceManager.getSpecificDevice(FormacarumRover.class, "rover");


System.out.println("Initialized OK, waiting for un-pause");
while(dyio.getValue(23)>0){
	ThreadUtil.wait(10);// wait in pause state
}
System.out.println("Starting run!");

rover.DriveStraight(10000, 15.0);
ThreadUtil.wait(15000);
rover.SetDriveVelocity(0);
boolean b = false;
while(true){
	if(dyio.getValue(23)==0){
		
		b=!b;
		System.out.println("Running Seek");
		//we are unpaused for this loop, run robot code
		ScriptingEngineWidget.inlineFileScriptRun(new File(System.getProperty("user.home")+"/git/9bbfbcae11130cdd4c3d/SeekObjects.groovy"), null);
		rover.DriveArc(b?1:-1*1000,b?1:-1*90, 5);
		ThreadUtil.wait(10000);
	}
	ThreadUtil.wait(100);
}

RoverStartup.groovy

import com.neuronrobotics.sdk.serial.SerialConnection; // Needed for serial.
import com.neuronrobotics.jniloader.OpenCVImageProvider
import com.neuronrobotics.jniloader.SalientDetector
import com.neuronrobotics.replicator.driver.BowlerBoardDevice; // Bowler talk.

import gnu.io.NRSerialPort; // Needed for the laser range finder.

import com.neuronrobotics.addons.driving.AckermanConfiguration
import com.neuronrobotics.addons.driving.AckermanDefaultKinematics;
import com.neuronrobotics.addons.driving.DataPoint;
import com.neuronrobotics.addons.driving.HokuyoURGDevice; // Needed for the laser range finder.
import com.neuronrobotics.addons.driving.IAckermanBotKinematics;
import com.neuronrobotics.addons.driving.URG2Packet;
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.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.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 org.apache.commons.io.IOUtils;
import org.reactfx.util.FxTimer;

boolean useVirtual = false;
//useVirtual=true;

public class ComputedGeometricModel  implements DhInverseSolver{
	private DHChain dhChain;
	public ComputedGeometricModel(DHChain dhChain){
		this.dhChain=dhChain;
	}
	private static final double M_PI = Math.PI;
	public double[] inverseKinematics(TransformNR target,double[] jointSpaceVector ) {
		
		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

		
		 // 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(0).getD(),
							 target.getRotation());
		 double l1 = dhChain.getLinks().get(1).getR();// First link length
		 double l2 = dhChain.getLinks().get(2).getR();

		 double xSet = overGripper.getX();
		 double ySet = overGripper.getY();
		 double zSet = overGripper.getZ();


		
		 double vect = Math.sqrt(xSet*xSet+ySet*ySet+zSet*zSet)
		 double xYvect = Math.sqrt(xSet*xSet+ySet*ySet);
		 
		
		 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);
		
		 System.out.println("Orentation: "+Math.toDegrees(orentation)+", Links: ,"+l1+","+l2+" vector distance: "+vect+", z: "+zSet);
		 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] = (180-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
		 //copy over remaining links so they do not move
		 for(int i=5;i<inv.length;i++){
			 inv[i]=jointSpaceVector[i];
		 }
		 
		return inv;
	}
}

DyIO dyio=null;
if(DeviceManager.getSpecificDevice(DyIO.class, "dyio")==null){
	// Adding in the DyIO.
	if(useVirtual)
		dyio = new DyIO(ConnectionDialog.promptConnection()); // This is the DyIO to talk to.
	if(dyio==null ){
		dyio = new DyIO(new SerialConnection("/dev/BowlerDevice.74F7260D0500")); // This is the DyIO to talk to.
	}
	dyio.connect(); // Connect to it.
	DeviceManager.addConnection(dyio,"dyio");
}else{
	dyio =(DyIO)DeviceManager.getSpecificDevice(DyIO.class, "dyio")
}


//OpenCVImageProvider clawCam;
//if(DeviceManager.getSpecificDevice(OpenCVImageProvider.class, "clawCam")==null){
//	// Adding the camera
//	clawCam = new OpenCVImageProvider(0);
//	DeviceManager.addConnection(clawCam,"clawCam");
//}else{
//	clawCam = (OpenCVImageProvider)DeviceManager.getSpecificDevice(OpenCVImageProvider.class, "clawCam");
//}
String xmlContent=null;
//define the file to load
//xmlContent = ScriptingEngineWidget.codeFromGistID("9bbfbcae11130cdd4c3d","trobotServo.xml")[0];
//Or use a local file
if(xmlContent==null)
	 xmlContent=new String(Files.readAllBytes(Paths.get(System.getProperty("user.home")+"/git/9bbfbcae11130cdd4c3d/trobotServo.xml")));
	 
System.err.println("Starting Model")
//Create the model
DHParameterKinematics model=null;
if(DeviceManager.getSpecificDevice(DHParameterKinematics.class, "DHArm")==null){
	 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()));
	DeviceManager.addConnection(model,"DHArm");
}else{
	model = (DHParameterKinematics)DeviceManager.getSpecificDevice(DHParameterKinematics.class, "DHArm");
}


//Return new model to UI
//Creating a list of objects, one for each link
ArrayList<Object> links = new ArrayList<Object>();

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).toCSG();
	cube=cube.transformed(new Transform().translateX(-x/2));
	cube=cube.transformed(new Transform().translateY(-y/2));
	//add listner to axis
	cube.setManipulator(dh.getListener());
	// add ax to list of objects to be returned
	links.add(cube);
}

return links;

SeekObjects.groovy

import java.awt.image.BufferedImage
import java.io.File;

import com.neuronrobotics.bowlerstudio.robots.FormacarumRover
import com.neuronrobotics.bowlerstudio.robots.RoverDataTable
import com.neuronrobotics.bowlerstudio.scripting.ScriptingEngineWidget;
import com.neuronrobotics.jniloader.Detection
import com.neuronrobotics.jniloader.OpenCVImageProvider
import com.neuronrobotics.jniloader.SalientDetector
import com.neuronrobotics.sdk.addons.kinematics.DHParameterKinematics
import com.neuronrobotics.sdk.common.DeviceManager;
import com.neuronrobotics.sdk.dyio.DyIO
import com.neuronrobotics.sdk.dyio.peripherals.ServoChannel
import com.neuronrobotics.sdk.util.ThreadUtil;
import com.neuronrobotics.jniloader.AbstractImageProvider;



OpenCVImageProvider clawCam = (OpenCVImageProvider)DeviceManager.getSpecificDevice(OpenCVImageProvider.class, "clawCam");
DHParameterKinematics model= (DHParameterKinematics)DeviceManager.getSpecificDevice(DHParameterKinematics.class, "DHArm"); // control of arm;
RoverDataTable datatable = (RoverDataTable)DeviceManager.getSpecificDevice(RoverDataTable.class, "datatable");             // list of all the stuff returned, ever *************************
FormacarumRover rover = (FormacarumRover)DeviceManager.getSpecificDevice(FormacarumRover.class, "rover");
DyIO dyio =(DyIO)DeviceManager.getSpecificDevice(DyIO.class, "dyio")
BufferedImage inputImage = AbstractImageProvider.newBufferImage(640,480);
BufferedImage outImage = AbstractImageProvider.newBufferImage(640,480);


SalientDetector FindStuff = (SalientDetector)datatable.getSpecificDetection("WhiteCylindar").getDetector();


double [] start  = model.getCurrentJointSpaceVector();

start[0]=0;
start[1]=25;
start[2]=-30;
start[3]=-85;
start[4]=0;

model.setDesiredJointSpaceVector(start, 0)
// Adding the wrist-bend
ServoChannel wristBend = new ServoChannel (dyio.getChannel(9));
wristBend.SetPosition(41);
wristBend.flush();

ThreadUtil.wait(5000);
clawCam.captureNewImage(inputImage);

ArrayList<Detection> detect =  FindStuff.getObjects(inputImage, outImage)
double degPerStep=5;
double startStep=0;
double negStep=-90;
double posStep =90;

while(dyio.getValue(23)==0){

	long startScan = System.currentTimeMillis();
	
	while(detect==null || detect.size()==0){
		if(dyio.getValue(23)==0){
			clawCam.captureNewImage(inputImage);
			detect =  FindStuff.getObjects(inputImage, outImage)	
			startStep+=degPerStep;
			if(startStep>posStep || startStep<negStep)
				degPerStep*=-1;
			model.setDesiredJointAxisValue(0, startStep, 1);
			ThreadUtil.wait(20000)
		}	
		if( System.currentTimeMillis()-startScan<30000){
			return;
		}
	}
	if(detect.get(0).getConfidence() > 0.5){
	
		println "Objects Found!!"
		model.setDesiredJointAxisValue(3, -5, 1000)
		ThreadUtil.wait(20000)
		
		System.out.println("Running BITE!");
		ScriptingEngineWidget.inlineFileScriptRun(new File(System.getProperty("user.home")+"/git/090e52d5b30090ddd636/Bite.groovy"), null);
	}else{
		//
		//drive a bit
		rover.DriveArc(200,((detect.get(0).getX()-320)/640)* 10 , 1000);
		
	}
	
}

trobotServo.xml

<root>
	
	<link>
		<name>basePan</name>
		    <deviceName>dyio</deviceName>
		<type>servo-rotory</type>
		<index>11</index>
		<scale>.796</scale>
		<upperLimit>196</upperLimit>
		<lowerLimit>41</lowerLimit>
		<isLatch>true</isLatch>
		<indexLatch>189</indexLatch>
		<isStopOnLatch>false</isStopOnLatch>	
		<homingTPS>500</homingTPS>
		<DHParameters>
		    <Delta>13</Delta>
		    <Theta>180</Theta>
		    <Radius>32</Radius>
		    <Alpha>-90</Alpha>
		</DHParameters>	
	</link>
	<link>
		<name>baseTilt</name>
		    <deviceName>dyio</deviceName>
		<type>servo-rotory</type>
		<index>10</index>
		<scale>.796</scale>
		<upperLimit>205</upperLimit>
		<lowerLimit>41</lowerLimit>
		<isLatch>true</isLatch>
		<indexLatch>90</indexLatch>
		<isStopOnLatch>false</isStopOnLatch>
		<homingTPS>500</homingTPS>
		<DHParameters>
		    <Delta>25</Delta>
		    <Theta>-90</Theta>
		    <Radius>93</Radius>
		    <Alpha>180</Alpha>
		</DHParameters>			
	</link>
	
	<link>
		<name>elbow</name>
		    <deviceName>dyio</deviceName>
		<type>servo-rotory</type>
		<index>9</index>
		<scale>.796</scale>
		<upperLimit>205</upperLimit>
		<lowerLimit>41</lowerLimit>
		<isLatch>true</isLatch>
		<indexLatch>189</indexLatch>
		<isStopOnLatch>false</isStopOnLatch>
		<homingTPS>500</homingTPS>
		<DHParameters>
		    <Delta>11</Delta>
		    <Theta>90</Theta>
		    <Radius>24</Radius>
		    <Alpha>90</Alpha>
		</DHParameters>				
	</link>
	
	<link>
		<name>wrist1</name>
		    <deviceName>dyio</deviceName>
		<type>servo-rotory</type>
		<index>8</index>
		<scale>.756</scale>
		<upperLimit>197</upperLimit>
		<lowerLimit>0</lowerLimit>
		<isLatch>true</isLatch>
		<indexLatch>111</indexLatch>
		<isStopOnLatch>false</isStopOnLatch>	
		<homingTPS>500</homingTPS>
		<DHParameters>
		    <Delta>128</Delta>
		    <Theta>-90</Theta>
		    <Radius>0</Radius>
		    <Alpha>90</Alpha>
		</DHParameters>			
	</link>
	
	<link>
		<name>wrist2</name>
		    <deviceName>dyio</deviceName>
		<type>servo-rotory</type>
		<index>7</index>
		<scale>.756</scale>
		<upperLimit>240</upperLimit>
		<lowerLimit>0</lowerLimit>
		<isLatch>true</isLatch>
		<indexLatch>121</indexLatch>
		<isStopOnLatch>false</isStopOnLatch>	
		<homingTPS>500</homingTPS>
		<DHParameters>
		    <Delta>0</Delta>
		    <Theta>0</Theta>
		    <Radius>0</Radius>
		    <Alpha>-90</Alpha>
		</DHParameters>			
	</link>
	<link>
		<name>wrist3</name>
		    <deviceName>dyio</deviceName>
		<type>servo-rotory</type>
		<index>6</index>
		<scale>.756</scale>
		<upperLimit>255</upperLimit>
		<lowerLimit>0</lowerLimit>
		<isLatch>true</isLatch>
		<indexLatch>171</indexLatch>
		<isStopOnLatch>false</isStopOnLatch>	
		<homingTPS>500</homingTPS>
		<DHParameters>
		    <Delta>25</Delta>
		    <Theta>90</Theta>
		    <Radius>0</Radius>
		    <Alpha>0</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>