Skip to content

Instantly share code, notes, and snippets.

@MazonDel

MazonDel/customdh.groovy

Forked from madhephaestus/customdh.groovy
Last active August 29, 2015 14:22
Show Gist options
  • Save MazonDel/e0c6a0f68430ebd6838c to your computer and use it in GitHub Desktop.
Save MazonDel/e0c6a0f68430ebd6838c to your computer and use it in GitHub Desktop.
Download ZIP
Raw
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;
Raw
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>-->
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment