Skip to content

Instantly share code, notes, and snippets.

@madhephaestus

madhephaestus/customdh.groovy

Last active July 25, 2016 22:31
Show Gist options
  • Save madhephaestus/0e6454891a3b3f7c8f28 to your computer and use it in GitHub Desktop.
Save madhephaestus/0e6454891a3b3f7c8f28 to your computer and use it in GitHub Desktop.
Download ZIP
An example of making custom kinematics implementations
Raw
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()));
Raw
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>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment