customdh.groovy

import com.neuronrobotics.sdk.serial.SerialConnection; // Needed for serial.
import com.neuronrobotics.imageprovider.OpenCVImageProvider
import com.neuronrobotics.imageprovider.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.ScriptingEngine;
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;

import Jama.Matrix;

boolean useVirtual = false;
//useVirtual=true;

public class MyComputedModel  implements DhInverseSolver{
	private DHChain dhChain;
	public MyComputedModel(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];

		//long start = System.nanoTime();

		// Placeholders for Pseudo Jacobian calculations.
		Matrix jacobian =  jointSpaceVector;
		Matrix Jt
    		Matrix Jstore
    		Matrix Jp

		// Calculate Pseudo Jacobian.  J^+ = (J^T * J)^-1 * J^T
    		Jt = jacobian.transpose()
    		Jstore = Jt.times(jacobian)
    		Jstore = Jstore.inverse()
    		Jp = Jstore.times(Jt)

    		// Get difference in time (in seconds) for calculating required velocities.
		long end = System.nanoTime();
		long deltaT = (end - start) / 1000000;

		// Get target position and orientation in task space, set up to calculate joint space velocity.
        	Matrix taskSpaceVel = new Matrix(6);
        	taskSpaceVel.set(0, 0, target.getX())
       	taskSpaceVel.set(1, 0, target.getY())
        	taskSpaceVel.set(2, 0, target.getZ())
        	RotationNR rot = target.getRotation();
        	taskSpaceVel.set(3, 0, rot.getRotationTilt())
        	taskSpaceVel.set(4, 0, rot.getRotationElevation())
        	taskSpaceVel.set(5, 0, rot.getRotationAzimuth())

        	// Solve for Joint Space Velocity, based on target joint space position and the timing function.
		taskSpaceVel = jointSpaceVel.times(1/deltaT);
		Matrix jointSpaceVel = Jp.times(taskSpaceVel);

		// this is an adhoc 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 analytical 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 parallel
		// We know the wrist twist will always be 0 for this model
		inv[4] = inv[0];//keep the camera orentation parallel 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;
	}
}


String xmlContent=null;
//define the file to load
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")));

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
	DeviceManager.addConnection(model,"DHArm");
}else{
	model = (DHParameterKinematics)DeviceManager.getSpecificDevice(DHParameterKinematics.class, "DHArm");
}

model.setInverseSolver(new MyComputedModel(model.getDhChain()));

//Return new model to UI
//Creating a list of objects, one for each link
ArrayList<Object> links = new ArrayList<Object>();
model.getCurrentJointSpaceVector();
int i=0;
for(DHLink dh : model.getDhChain().getLinks() ){
	model.setDesiredJointAxisValue(i++, 0, 1);
	System.out.println("Link D-H values = "+dh);
	// Create an axis to represent the link
	double y = dh.d>0?dh.d:2;
	double  x= dh.r>0?dh.r:2;

	CSG cube = new Cube(x,y,2).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());
	cube.setColor(Color.GOLD);
	// add ax to list of objects to be returned
	links.add(cube);
}

return links;

trobotServo.xml

<root>
<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>0</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>0</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>0</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.3125</scale>
	<upperLimit>255.0</upperLimit>
	<lowerLimit>0.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>175.0</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>175</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>0</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>0</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>
</root>