Skip to content

Instantly share code, notes, and snippets.

@madhephaestus

madhephaestus/DH-Solver.groovy

Last active March 6, 2017 22:26
Show Gist options
  • Save madhephaestus/a760a1753205775ad4bddbac5131418f to your computer and use it in GitHub Desktop.
Save madhephaestus/a760a1753205775ad4bddbac5131418f to your computer and use it in GitHub Desktop.
Download ZIP
Example Physics Walker
Raw
DH-Solver.groovy
import java.util.ArrayList;
import com.neuronrobotics.sdk.addons.kinematics.DHChain;
import com.neuronrobotics.sdk.addons.kinematics.DHLink;
import com.neuronrobotics.sdk.addons.kinematics.DhInverseSolver;
import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR;
import com.neuronrobotics.sdk.common.Log;
import Jama.Matrix;
return new DhInverseSolver() {
@Override
public double[] inverseKinematics(TransformNR target,
double[] jointSpaceVector,DHChain chain ) {
ArrayList<DHLink> links = chain.getLinks();
// THis is the jacobian for the given configuration
//Matrix jacobian = chain.getJacobian(jointSpaceVector);
Matrix taskSpacMatrix = target.getMatrixTransform();
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 d = links.get(1).getD()- links.get(2).getD();
double r = links.get(0).getR();
double lengthXYPlaneVect = Math.sqrt(Math.pow(target.getX(),2)+Math.pow(target.getY(),2));
double angleXYPlaneVect = Math.asin(target.getY()/lengthXYPlaneVect);
double angleRectangleAdjustedXY =Math.asin(d/lengthXYPlaneVect);
double lengthRectangleAdjustedXY = lengthXYPlaneVect* Math.cos(angleRectangleAdjustedXY)-r;
double orentation = angleXYPlaneVect-angleRectangleAdjustedXY;
if(Math.abs(Math.toDegrees(orentation))<0.01){
orentation=0;
}
double ySet = lengthRectangleAdjustedXY*Math.sin(orentation);
double xSet = lengthRectangleAdjustedXY*Math.cos(orentation);
double zSet = target.getZ() - links.get(0).getD();
if(links.size()>4){
zSet+=links.get(4).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 = links.get(1).getR();// First link length
double l2 = links.get(2).getR();
double vect = Math.sqrt(xSet*xSet+ySet*ySet+zSet*zSet);
/**
println ( "TO: "+target);
println ( "Trangular TO: "+overGripper);
println ( "lengthXYPlaneVect: "+lengthXYPlaneVect);
println( "angleXYPlaneVect: "+Math.toDegrees(angleXYPlaneVect));
println( "angleRectangleAdjustedXY: "+Math.toDegrees(angleRectangleAdjustedXY));
println( "lengthRectangleAdjustedXY: "+lengthRectangleAdjustedXY);
println( "r: "+r);
println( "d: "+d);
println( "x Correction: "+xSet);
println( "y Correction: "+ySet);
println( "Orentation: "+Math.toDegrees(orentation));
println( "z: "+zSet);
*/
if (vect > l1+l2 || vect<0 ||lengthRectangleAdjustedXY<0 ) {
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.0*b*c));
double B =Math.acos((Math.pow(c,2)+ Math.pow(a,2) - Math.pow(b,2)) / (2.0*a*c));
double C =Math.PI-A-B;//Rule of triangles
double elevation = Math.asin(zSet/vect);
/**
println( "vect: "+vect);
println( "A: "+Math.toDegrees(A));
println( "elevation: "+Math.toDegrees(elevation));
println( "l1 from x/y plane: "+Math.toDegrees(A+elevation));
println( "l2 from l1: ( C ) "+Math.toDegrees(C));
*/
inv[0] = Math.toDegrees(orentation);
inv[1] = -Math.toDegrees((A+elevation+links.get(1).getTheta()));
inv[2] = -(Math.toDegrees(C))+Math.toDegrees(links.get(2).getTheta());// offset for kinematics
if(links.size()>3)
inv[3] =(inv[1] -inv[2]);// keep it parallell
// We know the wrist twist will always be 0 for this model
if(links.size()>4)
inv[4] = inv[0];//keep the tool orentation paralell from the base
for(int i=0;i<inv.length;i++){
if(Math.abs(inv[i]) < 0.01){
inv[i]=0;
}
// println( "Link#"+i+" is set to "+inv[i]);
}
int i=3;
if(links.size()>3)
i=5;
//copy over remaining links so they do not move
for(;i<inv.length && i<jointSpaceVector.length ;i++){
inv[i]=jointSpaceVector[i];
}
return inv;
}
};
Raw
PhysicsOnlyArm.xml
<root>
<mobilebase>
<driveType>walking</driveType>
<cadEngine>
<git>https://gist.github.com/bcb4760a449190206170.git</git>
<file>ThreeDPrintCad.groovy</file>
</cadEngine>
<driveEngine>
<git>https://gist.github.com/375b47a8e071608688a1.git</git>
<file>Walking.groovy</file>
</driveEngine>
<name>PhysicsOnlyArm</name>
<leg>
<name>DefaultLeg</name>
<cadEngine>
<git>https://gist.github.com/bcb4760a449190206170.git</git>
<file>ThreeDPrintCad.groovy</file>
</cadEngine>
<kinematics>
<git>https://gist.github.com/375b47a8e071608688a1.git</git>
<file>DH-Solver.groovy</file>
</kinematics>
<link>
<name>basePan</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>3</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>105</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<mass>0.01</mass>
<centerOfMassFromCentroid> <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></centerOfMassFromCentroid>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.01</Theta>
<Radius>60.0</Radius>
<Alpha>-89.97</Alpha>
</DHParameters>
</link>
<link>
<name>baseTilt</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>0</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>97</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<mass>0.01</mass>
<centerOfMassFromCentroid> <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></centerOfMassFromCentroid>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.01</Theta>
<Radius>60.0</Radius>
<Alpha>0.01</Alpha>
</DHParameters>
</link>
<link>
<name>elbow</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>1</index>
<scale>0.33</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>15.223880597014928</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>128.0</staticOffset>
<isLatch>true</isLatch>
<indexLatch>145</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<mass>0.01</mass>
<centerOfMassFromCentroid> <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></centerOfMassFromCentroid>
<DHParameters>
<Delta>0.0</Delta>
<Theta>90.01</Theta>
<Radius>80.0</Radius>
<Alpha>0.01</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>-75.0</x>
<y>66.66666666666663</y>
<z>-3.1456319031046094E-14</z>
<rotw>0.702661135646815</rotw>
<rotx>0.07916646039534943</rotx>
<roty>0.7026611356468148</roty>
<rotz>-0.07916646039534948</rotz>
</baseToZframe>
</leg>
<leg>
<name>DefaultLeg2</name>
<cadEngine>
<git>https://gist.github.com/bcb4760a449190206170.git</git>
<file>ThreeDPrintCad.groovy</file>
</cadEngine>
<kinematics>
<git>https://gist.github.com/375b47a8e071608688a1.git</git>
<file>DH-Solver.groovy</file>
</kinematics>
<link>
<name>basePan</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>4</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>105</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<mass>0.01</mass>
<centerOfMassFromCentroid> <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></centerOfMassFromCentroid>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.01</Theta>
<Radius>60.0</Radius>
<Alpha>-89.97</Alpha>
</DHParameters>
</link>
<link>
<name>baseTilt</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>5</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>97</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<mass>0.01</mass>
<centerOfMassFromCentroid> <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></centerOfMassFromCentroid>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.01</Theta>
<Radius>60.0</Radius>
<Alpha>0.01</Alpha>
</DHParameters>
</link>
<link>
<name>elbow</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>6</index>
<scale>0.33</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>15.223880597014928</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>128.0</staticOffset>
<isLatch>true</isLatch>
<indexLatch>145</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<mass>0.01</mass>
<centerOfMassFromCentroid> <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></centerOfMassFromCentroid>
<DHParameters>
<Delta>0.0</Delta>
<Theta>90.01</Theta>
<Radius>80.0</Radius>
<Alpha>0.01</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>75.0</x>
<y>-33.333333333333314</y>
<z>2.405483220021172E-14</z>
<rotw>0.07925844130123613</rotw>
<rotx>-0.7026507663714437</rotx>
<roty>0.07925844130121582</roty>
<rotz>0.7026507663714434</rotz>
</baseToZframe>
</leg>
<leg>
<name>DefaultLeg3</name>
<cadEngine>
<git>https://gist.github.com/bcb4760a449190206170.git</git>
<file>ThreeDPrintCad.groovy</file>
</cadEngine>
<kinematics>
<git>https://gist.github.com/375b47a8e071608688a1.git</git>
<file>DH-Solver.groovy</file>
</kinematics>
<link>
<name>basePan</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>7</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>105</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<mass>0.01</mass>
<centerOfMassFromCentroid> <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></centerOfMassFromCentroid>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.01</Theta>
<Radius>60.0</Radius>
<Alpha>-89.97</Alpha>
</DHParameters>
</link>
<link>
<name>baseTilt</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>8</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>97</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<mass>0.01</mass>
<centerOfMassFromCentroid> <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></centerOfMassFromCentroid>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.01</Theta>
<Radius>60.0</Radius>
<Alpha>0.01</Alpha>
</DHParameters>
</link>
<link>
<name>elbow</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>12</index>
<scale>0.33</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>15.223880597014928</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>128.0</staticOffset>
<isLatch>true</isLatch>
<indexLatch>145</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<mass>0.01</mass>
<centerOfMassFromCentroid> <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></centerOfMassFromCentroid>
<DHParameters>
<Delta>0.0</Delta>
<Theta>90.01</Theta>
<Radius>80.0</Radius>
<Alpha>0.01</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>75.0</x>
<y>33.333333333333314</y>
<z>9.251858538542975E-15</z>
<rotw>0.07919711971273516</rotw>
<rotx>0.7026576806876241</rotx>
<roty>0.07919711971275165</roty>
<rotz>-0.7026576806876244</rotz>
</baseToZframe>
</leg>
<leg>
<name>DefaultLeg4</name>
<cadEngine>
<git>https://gist.github.com/bcb4760a449190206170.git</git>
<file>ThreeDPrintCad.groovy</file>
</cadEngine>
<kinematics>
<git>https://gist.github.com/375b47a8e071608688a1.git</git>
<file>DH-Solver.groovy</file>
</kinematics>
<link>
<name>basePan</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>13</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>105</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<mass>0.01</mass>
<centerOfMassFromCentroid> <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></centerOfMassFromCentroid>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>60.0</Radius>
<Alpha>-90.0</Alpha>
</DHParameters>
</link>
<link>
<name>baseTilt</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>14</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>97</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<mass>0.01</mass>
<centerOfMassFromCentroid> <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></centerOfMassFromCentroid>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>60.0</Radius>
<Alpha>0.0</Alpha>
</DHParameters>
</link>
<link>
<name>elbow</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>15</index>
<scale>0.33</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>15.223880597014928</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>128.0</staticOffset>
<isLatch>true</isLatch>
<indexLatch>145</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<mass>0.01</mass>
<centerOfMassFromCentroid> <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></centerOfMassFromCentroid>
<DHParameters>
<Delta>0.0</Delta>
<Theta>90.0</Theta>
<Radius>80.0</Radius>
<Alpha>0.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>-75.0</x>
<y>-66.66666666666663</y>
<z>-1.8503717077086026E-15</z>
<rotw>0.7051320916686237</rotw>
<rotx>-0.05280845859359904</rotx>
<roty>0.7051320916686235</roty>
<rotz>0.052808458593599034</rotz>
</baseToZframe>
</leg>
<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>
<mass>0.01</mass>
<centerOfMassFromCentroid> <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></centerOfMassFromCentroid>
</mobilebase>
</root>
Raw
Walking.groovy
import java.time.Duration;
import java.util.ArrayList;
import javafx.application.Platform;
import org.reactfx.util.FxTimer;
import com.neuronrobotics.sdk.addons.kinematics.DHParameterKinematics;
import com.neuronrobotics.sdk.addons.kinematics.MobileBase;
import com.neuronrobotics.sdk.addons.kinematics.math.RotationNR;
import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR;
import com.neuronrobotics.sdk.util.ThreadUtil;
import com.neuronrobotics.sdk.addons.kinematics.IDriveEngine;
double stepOverHeight=5;
long stepOverTime=100;
Double zLock=-15;
Closure calcHome = { DHParameterKinematics leg ->
TransformNR h=leg.calcHome()
TransformNR tr = leg.forwardOffset(new TransformNR())
tr.setZ(zLock)
return tr;
}
boolean usePhysicsToMove = true;
return ScriptingEngine
.gitScriptRun(
"https://github.com/madhephaestus/carl-the-hexapod.git", // git location of the library
"GenericWalkingEngine.groovy" , // file to load
[stepOverHeight,stepOverTime,zLock,calcHome,usePhysicsToMove]
)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment