Skip to content

Instantly share code, notes, and snippets.

@NotOctogonapus

NotOctogonapus/DefaultDhSolver.groovy

Created August 19, 2019 17:18
Show Gist options
  • Save NotOctogonapus/d59d758e321d49c5d95932ef01e5f8b2 to your computer and use it in GitHub Desktop.
Save NotOctogonapus/d59d758e321d49c5d95932ef01e5f8b2 to your computer and use it in GitHub Desktop.
Download ZIP
HephaestusWorkCell_copy copy of HephaestusWorkCell
Raw
DefaultDhSolver.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();
if(links.size()<3){
return jointSpaceVector
}
// 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];
if(linkNum<3){
for(int i=i;i<linkNum;i++){
inv[i]=jointSpaceVector[i]+0.0001
}
}
// 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()==5){
double tipAngulationSum =Math.toDegrees( links.get(1).getTheta()+
links.get(2).getTheta()+
links.get(4).getTheta())
//println "Tip angulation orentation = "+tipAngulationSum
if(tipAngulationSum==90)
zSet+=links.get(4).getD();
else{
double tipySet = links.get(4).getR()*Math.sin(orentation);
double tipxSet = links.get(4).getR()*Math.cos(orentation);
//println "5 links back Setting tip x="+tipxSet+" y="+tipySet
xSet-=tipxSet
ySet-=tipySet
}
}
if(links.size()==4){
double tipAngulationSum =Math.toDegrees( links.get(1).getTheta()+
links.get(2).getTheta()+
links.get(3).getTheta())
//println "Tip angulation orentation = "+tipAngulationSum
if(tipAngulationSum==90)
zSet+=links.get(3).getR();
else{
double tipySet = links.get(3).getR()*Math.sin(orentation);
double tipxSet = links.get(3).getR()*Math.cos(orentation);
//println "4 links back Setting tip x="+tipxSet+" y="+tipySet
xSet-=tipxSet
ySet-=tipySet
}
}
// 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: "+Math.toDegrees(C));
*/
inv[0] = Math.toDegrees(orentation);
inv[1] = -Math.toDegrees((A+elevation+links.get(1).getTheta()));
if((int)links.get(1).getAlpha() ==180){
inv[2] = (Math.toDegrees(C))-180-//interior angle of the triangle, map to external angle
Math.toDegrees(links.get(2).getTheta());// offset for kinematics
}
if((int)links.get(1).getAlpha() ==0){
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
HephaestusWorkCell_copy.xml
<root>
<mobilebase>
<cadEngine>
<git>https://gist.github.com/d59d758e321d49c5d95932ef01e5f8b2.git</git>
<file>seaLinkGen.groovy</file>
</cadEngine>
<driveEngine>
<git>https://gist.github.com/d59d758e321d49c5d95932ef01e5f8b2.git</git>
<file>WalkingDriveEngine.groovy</file>
</driveEngine>
<name>HephaestusWorkCell_copy</name>
<appendage>
<name>HephaestustArm</name>
<cadEngine>
<git>https://gist.github.com/d59d758e321d49c5d95932ef01e5f8b2.git</git>
<file>seaLinkGen.groovy</file>
</cadEngine>
<kinematics>
<git>https://gist.github.com/d59d758e321d49c5d95932ef01e5f8b2.git</git>
<file>DefaultDhSolver.groovy</file>
</kinematics>
<link>
<name>basePan</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>11</index>
<scale>0.1</scale>
<upperLimit>856.0</upperLimit>
<lowerLimit>-875.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>0.0</staticOffset>
<isLatch>true</isLatch>
<indexLatch>235</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<vitamins>
<vitamin>
<name>electroMechanical</name>
<type>hobbyServo</type>
<id>hv5932mg</id>
</vitamin>
<vitamin>
<name>shaft</name>
<type>hobbyServoHorn</type>
<id>hv6214mg_6</id>
</vitamin>
</vitamins>
<passive>false</passive>
<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>
<imuFromCentroid> <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></imuFromCentroid>
<DHParameters>
<Delta>135.0</Delta>
<Theta>0.0</Theta>
<Radius>0.0</Radius>
<Alpha>-90.0</Alpha>
</DHParameters>
</link>
<link>
<name>baseTilt</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>10</index>
<scale>0.1</scale>
<upperLimit>95.0</upperLimit>
<lowerLimit>-1008.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>0.0</staticOffset>
<isLatch>true</isLatch>
<indexLatch>128</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<vitamins>
<vitamin>
<name>electroMechanical</name>
<type>hobbyServo</type>
<id>hv5932mg</id>
</vitamin>
<vitamin>
<name>shaft</name>
<type>hobbyServoHorn</type>
<id>hv6214mg_6</id>
</vitamin>
</vitamins>
<passive>false</passive>
<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>
<imuFromCentroid> <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></imuFromCentroid>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>175.0</Radius>
<Alpha>0.0</Alpha>
</DHParameters>
</link>
<link>
<name>elbow</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>9</index>
<scale>0.1</scale>
<upperLimit>304.0</upperLimit>
<lowerLimit>-2093.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>0.0</staticOffset>
<isLatch>true</isLatch>
<indexLatch>121</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<vitamins>
<vitamin>
<name>electroMechanical</name>
<type>hobbyServo</type>
<id>hv5932mg</id>
</vitamin>
<vitamin>
<name>shaft</name>
<type>hobbyServoHorn</type>
<id>hv6214mg_6</id>
</vitamin>
</vitamins>
<passive>false</passive>
<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>
<imuFromCentroid> <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></imuFromCentroid>
<DHParameters>
<Delta>0.0</Delta>
<Theta>90.0</Theta>
<Radius>169.28</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>0.0</x>
<y>0.0</y>
<z>65.0</z>
<rotw>1.0</rotw>
<rotx>-2.7755575615628907E-17</rotx>
<roty>-2.775557561562889E-17</roty>
<rotz>8.326672684688674E-17</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>
<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>
<imuFromCentroid> <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></imuFromCentroid>
<vitamins>
</vitamins>
</mobilebase>
</root>
Raw
seaLinkGen.groovy
return ScriptingEngine.gitScriptRun(
"https://github.com/madhephaestus/SeriesElasticActuator.git", // git location of the library
"LinkedGearedCadGen.groovy" , // file to load
// Parameters passed to the funcetion
[ [36,// Number of teeth gear a link 0
84],// Number of teeth gear b link 0
[60,// Number of teeth gear a link 1
60],// Number of teeth gear b link 1
[36,// Number of teeth gear a link 2
84],// Number of teeth gear b link 2
]
)
Raw
WalkingDriveEngine.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=200;
Double zLock=5;
Closure calcHome = { DHParameterKinematics leg ->
TransformNR h=leg.calcHome()
TransformNR legRoot= leg.getRobotToFiducialTransform()
TransformNR tr = leg.forwardOffset(new TransformNR())
tr.setZ(zLock)
//Bambi-on-ice the legs a bit
if(legRoot.getY()>0){
//tr.translateY(-5)
}else{
//tr.translateY(5)
}
return h;
}
boolean usePhysicsToMove = true;
long stepCycleTime =5000
long walkingTimeout =stepCycleTime*2
int numStepCycleGroups = 2
double standardHeadTailAngle = -20
double staticPanOffset = 10
double coriolisGain = 1
boolean headStable = false
double maxBodyDisplacementPerStep = 20
double minBodyDisplacementPerStep = 15
def ar = [stepOverHeight,
stepOverTime,
zLock,
calcHome,
usePhysicsToMove,
stepCycleTime,
numStepCycleGroups,
standardHeadTailAngle,
staticPanOffset,
coriolisGain,
headStable,
maxBodyDisplacementPerStep,
minBodyDisplacementPerStep,
walkingTimeout]
return ScriptingEngine
.gitScriptRun(
"https://github.com/CommonWealthRobotics/SmallKat.git", // git location of the library
"Bowler/DynamicWalking.groovy" , // file to load
ar
)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment